Whole-Body Ergodic Exploration with a Manipulator Using Diffusion

Accepted to IEEE RA-L

 Cem Bilaloglu*, Tobias Löw and Sylvain Calinon

Abstract

This paper presents a whole-body robot control method for exploring and probing a given region of interest. The ergodic control formalism behind such an exploration behavior consists of matching the time-averaged statistics of a robot trajectory with the spatial statistics of the target distribution. Most existing ergodic control approaches assume the robots/sensors as individual point agents moving in space. We introduce an approach exploiting multiple kinematically constrained agents on the whole-body of a robotic manipulator, where a consensus among the agents is found for generating control actions. To do so, we exploit an existing ergodic control formulation called heat equation-driven area coverage (HEDAC), combining local and global exploration on a potential field resulting from heat diffusion. Our approach extends HEDAC to applications where robots have multiple sensors on the whole-body (such as tactile skin) and use all sensors to optimally explore the given region. We show that our approach increases the exploration performance in terms of ergodicity and scales well to real-world problems using agents distributed on multiple robot links. We compare our method with HEDAC in kinematic simulation and demonstrate the applicability of an online exploration task with a 7-axis Franka Emika robot.

Summary

Whole-body exploration of a target distribution using the last three links of the robot manipulator. In kinematic simulation, the exploration target is given in red. In real-world experiment, the robot explores the cube region in dashed lines for localizing a target object (tennis ball whose location is unknown). Blue, turquoise, and purple spheres are the virtual agents constrained to the 5-th, 6-th, and 7-th links, respectively. The green and yellow arrows show the net virtual force and torque acting on each link's center of mass calculated by our agent weighing strategy. We further weight the net wrenches acting on the active links by their manipulability index to generate the consensus control action of the robot.

Contributions

We present the first whole-body ergodic exploration method and the first three-dimensional control implementation of the HEDAC approach. We summarize our contributions as:


Method

HEDAC [1] method computes the potential field u(x,t) that will guide agents for ergodic exploration. Time-averaged coverage of the agent(s) c(x,t) at time t is subtracted from the target distribution p(x), and positive values corresponding to unexplored regions are squared and used as virtual heat source term s(x,t). The heat equation is then used for diffusing the potential field propagating information of unexplored regions to the agents. 

Unlike the original HEDAC, we solve the non-stationary diffusion equation to slow the diffusion to increase local exploration. Then, we combine local exploration and introduce weighting strategies on the agent and link levels to simplify reaching a consensus among the agents.

Decompose Whole-body to Virtual Exploration Agents

Combine Weighted Virtual Agents and Links


Results

2-D Manipulator Exploring 'X' shape using different virtual agent configurations

Active Configuration

Note that below animations are intentionally low-res to better show how the diffusion process takes place

1 Explicit time-step iteration

10 explicit time-step iteration

100 explicit time-step iteration

SMC (replayed 10x faster)

Stationary Diffusion

Favors global exploration (smoother exploration, slower computation)

Non-stationary Diffusion

Favors local exploration (better alignment, faster computation)

Single Configuration

Robot explores only with the tip of its last link

Passive Configuration

Robot explores by considering the coverage by its last link

Front View (Link 7 only)

Link 7 still explore the regions already explored by 5 and 6

Front View (Link 5,6 and 7)

Link 5 and 6 helps exploration (spends more time in target)

Isometric View (Link 7 only)

Link 7 still explore the regions alrady explored by 5 and 6

Isometric View (Link 5,6 and 7)

Link 5 and 6 helps exploration (spends more time in target)

Alternative Target Distribution

Top View (Link 7 only)

Isometric View (Link 7 only)

Link 7 still explore the regions already explored by 5 and 6

Front View (Link 7 only)

Right View (Link 7 only)

Real-world Experiments

Real-world experiment of the robot exploring the cube in dashed lines using its last three links, until either b) link 5, c) link 6 and d) link 7 contacts the target object

Supplementary Video 

RAL_cem_supplementary.mp4

Horizon Europe Projects 

This work was supported by the State Secretariat for Education, Research and Innovation in Switzerland for participation in the European Commission's Horizon Europe Program through the INTELLIMAN project (https://intelliman-project.eu/, HORIZON-CL4-Digital-Emerging Grant 101070136) and the SESTOSENSO project (http://sestosenso.eu/, HORIZON-CL4-Digital-Emerging Grant 101070310).