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:
increased sensor coverage by modeling the whole-body as a collection of virtual exploration agents constrained to the robot’s kinematic chain;
formulating a locally consistent exploration behavior in time and space, combining agents for global exploration;
controlling the robot with consensus among virtual agents and links by the introduced weighting strategies
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.mp4Horizon 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).