Learning from Demonstration (LfD) based on Movement Primitives (MPs) constitutes a robust paradigm for transferring human skills to robots. However, many existing approaches often rely on homoscedastic assumptions or parametric constraints, limiting the robot’s ability to modulate stiffness according to specific task variations. To address this, we present Gaussian Movement Primitives (GMP), a non-parametric framework that explicitly learns a time-varying heteroscedastic prior from demonstrations. By interpreting the inverse covariance as a dynamic stiffness matrix, our method introduces a precision-weighted fusion mechanism. This formulation ensures geometric fidelity at via-points (handling them as infinite-precision constraints) and enables the safe composition of conflicting skills by automatically prioritizing high-precision behaviors. The framework is further enhanced by a temporal alignment preprocessing stage and a local obstacle avoidance module. To validate its effectiveness, the proposed method was evaluated in simulation and on real robotic manipulators, demonstrating significant improvements in reproduction accuracy and adaptability when compared against various LfD algorithms based on Movement Primitives.

Robotic manipulation can involves multiple manipulators to complete a task. In those cases, the complexity of performing the task in a coordinated manner increases, requiring coordinated planning while avoiding collisions between robots and environmental elements. For these challenges, we propose a robotic arm control algorithm based on Learning from Demonstration to independently learn the tasks of each arm, followed by a graph-based communication method using Gaussian Belief Propagation. Our method enables the resolution of decoupled dual-arm tasks learned independently with-out requiring coordinated planning. The algorithm generates smooth, collision-free solutions between arms and environmental obstacles while ensuring efficient movements without the need for constant replanning. Its efficiency has been validated through experiments and comparisons against another multi-robot control method in simulation using PyBullet with two opposing IIWA robots, as well as a mobile robot with two UR3 arms, which has also been used for real-world testing. Code provided in https://adrianprados.github.io/GaussianBeliefPropagationDualArm/

Swarms composed of multiple unmanned aerial vehicles (UAVs) are emerging as a key tool in various application fields including aerial surveillance, urban search and rescue, and package delivery. In this context, trajectory planning remains one of the principal concerns regarding complex robotic applications. This paper proposes a 4D path planning algorithm based on the Fast Marching Square (FM$^2$) method for multi-UAV teams in high-dimensional 3D environments with multiple obstacles. The 4D-FM$^2$algorithm integrates a time-dependent speed function within the Fast Marching (FM) framework. The proposed algorithm was tested in a simulated urban scenario and results demonstrate that the algorithm effectively plans safe, optimal solutions to the shortest path problem for UAVs while avoiding obstacles and other drones, even in complex situations. Additionally, the algorithm excels in providing smooth speed profiles for the vehicles. Furthermore, the study also successfully evaluated the effect of various implementation parameters on the algorithm’s outcome, such as the number of concurrent missions and the velocity of the vehicles. Moreover, computational time was kept within acceptable limits, and the method demonstrated overall good performance in terms of air traffic flow. The main contribution of this work is the introduction of a novel 4D trajectory planning algorithm that implicitly incorporates UAV movement and speed and hence the spatio-temporal realization of the path during the planning phase, leveraging the light propagation phenomenon. This approach efficiently addresses the presence of other UAVs and environmental obstacles, thereby preventing collisions and avoiding unnecessary airspace blockages.

The exploration of unknown environments is a challenge in robotics. The proposed method approaches this problem by combining the Fast Marching Square path planning technique with the machine learning method called Gaussian processes (GP). The Fast Marching Square method is used to determine the most unexplored areas of the environment and to plan the path of the vehicle from the current position to the selected point. The GP model is used to obtain predictions about the unexplored regions of the environment based on the collected data so far during the exploration. The use of Unmanned Aerial Vehicles (UAVs) for exploration and surveillance has increased exponentially in the recent years, due to their sensor equipment capabilities and their versatility for flying over difficult terrain. By defining the weight each method has on the selection of the next point to explore, we can focus the UAV on the points with more interesting data defined by the user (i.e. bodies of water), the most unexplored regions, or a combination of both. We present an study on the influence of these weights on the mean absolute error (MAE) and predictive variance obtained from the GP model and test the algorithm on a real environment obtained from a satellite image. We show that we are able to generate an accurate depiction of the environment way faster than traditional methods such as the Boustrophedon.

This paper presents a rehabilitation wearable exoskeleton for wrist joint with two degrees of freedom (DOF), flexion-extension and adduction-abduction (radial and ulnar deviation), actuated with Shape Memory Alloy (SMA) based actuators. Thanks to this type of actuators, the proposed device presents a very light weight and noiseless operation, in comparison with similar devices. The preliminary results obtained over real tests with the wrist exoskeleton are presented. This prototype demonstrates that SMA actuator technology is a viable alternative when investigating possible improvement of rehabilitation robotic devices in terms of weight, size and cost.

Many proposed activities to be carried out by unmanned aerial vehicles (UAVs) in urban environments require a control over the altitude for different purposes. Energy saving and minimization of costs are some of these objectives. This work presents a method to impose a flight level in a mission planning carried out by a UAV in a 3D urban environment. The planning avoids all obstacles encountered in the environment and maintains a fixed flight level in the majority of the trajectory. The method used as planner is the Fast Marching Square (FM2) method, which includes two adjustment parameters. Depending on the values of these parameters, it is possible to introduce into the planning an altitude constraint, as well as to modify the smoothness of the trajectory and the safety margins from the obstacles. Several simulated experiments have been carried out in different situations obtaining very good results.

Bi-directional search is a widely used strategy to increase the success and convergence rates of sampling-based motion planning algorithms. Yet, few results are available that merge both bi-directional search and asymptotic optimality into existing optimal planners, such as PRM*, RRT*, and FMT*. The objective of this paper is to fill this gap. Specifically, this paper presents a bi-directional, sampling-based, asymptotically-optimal algorithm named Bi-directional FMT* (BFMT*) that extends the Fast Marching Tree (FMT*) algorithm to bi-directional search while preserving its key properties, chiefly lazy search and asymptotic optimality through convergence in probability. BFMT* performs a two-source, lazy dynamic programming recursion over a set of randomly-drawn samples, correspondingly generating two search trees: one in cost-to-come space from the initial configuration and another in cost-to-go space from the goal configuration. Numerical experiments illustrate the advantages of BFMT* over its unidirectional counterpart, as well as a number of other state-of-the-art planners.

Rhombic like vehicles are characterized for high maneuverability in cluttered environments. This type of vehicles will be used on remote handling operations of maintenance in the International Thermonuclear Experimental Reactor (ITER). Previous work was done in motion planning using Constrained Delaunay Triangulation for rhombic like vehicles operating in ITER. This paper shows that the integration of Fast Marching Square improves the motion planning methodology, decreasing also the computational effort, which can be applied not only in ITER but also in other complex and cluttered environments. Simulated results are presented comparing the initial and the improved motion planning.

This paper presents a novel methodology for planning the movements of a robotic hand when a precision grasp is going to be performed. The approach used is based on the standard Fast Marching Square (FM2) path planning method and its application to robot formations motion planning. In this case, the hand is considered to be a kinematic chain in which a mobile robot is located at every joint position. The robot formation is therefore deformable among the positions allowed by the mechanical limits of the joints. To perform a given precision grasp, the task is divided into two phases. First the hand approaches the object. FM2 is used to calculate a fast and smooth path towards the object to be grasped. While the hand is covering it, the formation updates its shape according to the map of velocities calculated in FM2. The second phase consists on performing the precision grasp. Every finger is modelled as a robot formation and a path is calculated for each fingertip so that they reach the grasping points on the object. The position of the joints of the fingers is computed using an inverse kinematics algorithm. Simulations show the usefulness of this approach thanks to a good performance of the approaching and the grasping tasks.

Facial gesture recognition is one of the main topics in HRI. We have developed a novel algorithm who allows to detect emotional states, like happiness, sadness or emotionless. A humanoid robot is able to detect these states with a ratio of success of 83% and interact in consequence. We use Active Appearance Models (AAMs) to determinate face features and classify the emotions using neural evolution, based on neural networks and differential evolution algorithm.

This paper presents a novel robotic learning technique based on Fast Marching Square (FM 2 ). This method, which we have called FM Learning, is based on incorporating previous experience to the path planning system of the robot by taking into account paths taught to the robot via kinesthetic teaching, this is, guiding manually the robot through the desired path. The method proposed ensures that the path planning is always a globally asymptotically stable system at the target point, considering the motion as a nonlinear autonomous dynamical system. The few parameters the algorithm has can be tuned to get different behaviours of the learning system. The method has been evaluated through a set of simulations and also tested in the mobile manipulator Manfred V2.

A new evolutionary-based algorithm is proposed to solve the robot manipulator optimal path generation problem. The following scenario is considered: given a learnt joint path describing a robot manipulator simple task in the Cartesian space, an optimal path is calculated when a different initial joint configuration is considered. The optimization problem is formulated as the minimization of both the end-effector pose error and the total joint displacement so as to ensure convergence towards the learnt path and a smooth joint motion. To solve the optimization problem an algorithm based on an evolutionary method called differential evolution (DE) is used. DE is a stochastic direct search optimization method based on the evolution of a candidate solution population in an iterative process of mutation, recombination, and selection. Since the algorithm does not require the use of the Jacobian matrix during the kinematic inversion, singularities problems are overcome. Results on the optimal path generation of a six degrees of freedom robot manipulator are also presented.

In this paper, a new methodology to build compact local maps in real time for outdoor robot navigation is presented. The environment information is obtained from a 3-D scanner laser. The navigation model, which is called traversable region model, is based on a Voronoi diagram technique, but adapted to large outdoor environments. The model obtained with this methodology allows a definition of safe trajectories that depend on the robot's capabilities and the terrain properties, and it will represent, in a topogeometric way, the environment as local and global maps. The application presented is validated in real outdoor environments with the robot called GOLIAT.

A new solution to the Simultaneous Localization and Modelling problem is presented. It is based on the stochastic search of solutions in the state space to the global localization problem by means of a differential evolution algorithm. A non linear evolutive filter, called Evolutive Localization Filter (ELF), searches stochastically along the state space for the best robot pose estimate. The proposed SLAM algorithm operates in two steps: in the first step the ELF filter is used at a local level to re-localize the robot based on the robot odometry, the laser scan at a given position and a local map where only a low number of the last scans have been integrated. In a second step the aligned laser measures together with the corrected robot poses are use to detect when the robot is revisiting a previously crossed area. Once a cycle is detected, the Evolutive Localization Filter is used again to re-estimate the robot poses in order to integrate the sensor measures in the global map of the environment. The algorithm has been tested in different environments to demonstrate the effectiveness, robustness and computational efficiency of the proposed approach.

This paper presents a new path planning method which operates in two steps. In the first step the safest areas in the environment are extracted by means of a Voronoi diagram. In the second step fast marching method is applied to the Voronoi extracted areas in order to obtain the shortest path. In this way the trajectory obtained is the shortest between the safe possible ones. This two step method combines speed and reliability, because the map dimensions is reduced to a unidimensional map and this map represents the safest areas in the environment for moving the robot

This paper presents a new sensor based global path planner which operates in two steps. In the first step the safest areas in the environment are extracted by means of a Voronoi diagram. In the second step fast marching method is applied to the Voronoi extracted areas in order to obtain the shortest path. In this way the trajectory obtained is the shortest between the safe possible ones. This two step method combines an extremely fast global planner operating on a simple sensor based environment modeling, while it operates at the sensor frequency. The main characteristics are speed and reliability, because the map dimensions are reduced to a unidimensional map and this map represents the safest areas in the environment for moving the robot

This paper presents a new path planning method based in the inverse of the logarithm of the distance transform and in the fast marching method. The distance transform of an image gives a grey scale that is darker near the obstacles and walls and more clear far from them and it is calculated via Voronoi diagram. The logarithm of the inverse of the distance transform imitates the repulsive electric potential from walls and obstacles. This method is very fast and reliable and the trajectories are similar to the human trajectories: smooth and not very close to obstacles and walls

In robotic service field, safety navigation in human environments is one of the most important goals. To navigate without collision, some kind of environment representation is needed. This paper presents a new methodology to build, in real-time, compact local maps for navigation in complex indoor environments. The map is called Traversable Regions Model (TRM) and is based on Voronoi diagram techniques. The new approach is the application of Voronoi techniques in 3D environments and the previous study of tr avers ability in indoor environments. The model built allows to define safe trajectories depending on the robot's capabilities and the terrain properties, because a previous traversability study is performed with the 3D data. The methodology presented is validated in real indoor environments with the mobile manipulator MANFRED, developed in our lab.

This paper shows the robustness path planning for mobile robots in indoors environments. The planning includes an accurate uncertainty in position analysis and the probability of success calculation as a path quality measurement. A topological and metric environment model, for the environment representation, is proposed. This model includes localization and risk transit zones. During the planning process, all the paths to complete a mission, are generated. Obtaining parameters such as: goal uncertainty, path feasibility, path distance and path uncertainty. These parameters allow the most adequate paths selection to fulfil a mission.

Simultaneous localization and mapping (SLAM) is one of the fundamental problems in mobile robots. In this paper we present a new SLAM algorithm in an unknown indoor environment. A local Voronoi diagram (LVD) is built from a laser scanner measurements, in order to estimate the robot position using its topological and geometrical characteristics. The proposed algorithm introduces a backward step, which revises all previous poses. This step is fundamental to obtain good results in large and cyclic indoor environments. Genetic algorithms are used in order to improve the estimated approach.

Sensor-based localization is one of the fundamental problems in mobile robots. We present a technique for online robot localization in an a priori known indoor environment. Our approach uses the local Voronoi diagram, generated from a laser range scan, to match it against the global Voronoi diagram of the robot's workspace. The result from this process is used to estimate the robot's position in the map or to correct the robot's odometry. Experiments with real data are presented to validate this algorithm.

To navigate in complex environments, an autonomous mobile robot needs to reach a compromise between the need for reacting to unexpected events and the need for having efficient and optimized trajectories. Sensor based path planning can be used to achieve this goal. We present a new sensor based method which consist of building up local Voronoi diagrams using measurements from a scanning laser. The space is divided into regular cells, and the Euclidean distance is calculated between each cell and the objects. The cells which are equidistant to several objects belong to the Voronoi diagram. Experimental results obtained by running this algorithm are also presented. These results show that our method can be used in sensor based path planning.

This paper presents a fuzzy perception planner that takes into account the time cost, the suitability of every landmark detection and the different uncertainties the robot encounters along its path for mobile robot localization. The sensor used is a camera with a motorized zoom on a pan and tilt platform and the artificial landmarks are circles detected through normalized gray scale correlation. An extended Kalman filter is used to correct the position and orientation of the vehicle. The resulting self-localization module has been integrated successfully in a more complicated navigation system.

We present an implementation of a reinforcement learning algorithm through the use of a special neural network topology, the AHC (adaptive heuristic critic). The AHC is used as a fusion supervisor of primitive behaviors in order to execute more complex robot behaviors, for example go to goal, surveillance or follow a path. The fusion supervisor is part of an architecture for the execution of mobile robot tasks which are composed of several primitive behaviors which act in a simultaneous or concurrent fashion. The architecture allows for learning to take place at the execution level, it incorporates the experience gained in executing primitive behaviors as well as the overall task. The implementation of this autonomous learning approach has been tested within OPMOR, a simulation environment for mobile robots and with our mobile platform, the UPM Robuter. Both, simulated and actual results are presented. The performance of the AHC neural network is adequate. Portions of this work has been implemented within the EEC ESPRIT 2483 PANORAMA Project.>