EHC-MM: Embodied Holistic Control for Mobile Manipulation

Mobile manipulation is extensively explored across various domains in recent years. Notably, Chitta et al. [11] utilize visual, tactile, and proprioceptive sensor data to execute pick-and-place tasks with the PR2 robot. In addition, HERB [12] enables a robotic agent to identify grasping targets and navigate through complex environments. These contributions lay the foundation for mobile manipulation, emphasizing the segmentation of manipulation and navigation tasks. However, this partitioning in the planning domain is inherently artificial and introduces unnecessary complexity into the planning space.

To address the challenge of whole-body control in mobile grasping, Haviland et al. [5] adapt NEO [13] from stationary to mobile manipulators, proposing a novel taskable reactive mobile manipulation system. They treat the arm and base degrees of freedom as a unified structure, significantly enhancing the speed and fluidity of motion.

Drawing inspiration from the Virtual Kinematic Chain (VKC) concept [14], Jiao et al. [4, 1] develop a Virtual Kinematic Chain framework for mobile manipulation tasks. This approach integrates the kinematics of the mobile base, robot arm, and manipulated object, simplifies task planning by defining abstract actions, and reduces complexity in describing intermediate poses. Implementing a task planner using PDDL with VKC shows significant improvements in planning efficiency, with experiments demonstrating the effectiveness of VKC-based planning in generating feasible motion plans and trajectories.

Previous work has introduced the concept of Embodied Control into optimization control of quadrupedal robots[15], swarm robots[16], soft robots[17], and human-robot interaction[18, 19]. This approach has streamlined control system design, enhancing robot adaptability and stability.

Regarding the distinct roles of different robot components in mobile manipulation, each part serves a specific function. Typically, the mobile base exhibits lower precision, on the order of centimeters, but facilitates spatial mobility. In contrast, the robot arm offers higher precision, operating at the millimeter level, albeit within a limited workspace, primarily for object manipulation tasks, as shown in Fig. 1

According to the chain rule in robot kinematics, the pose of the robot’s end-effector can be represented as:

$${}_{e}^{o}\mathbf{T}={}_{v}^{o}\mathbf{T}(x,y,\theta)\cdot{}_{b}^{v}\mathbf{T}\cdot{}_{a}^{b}\mathbf{T}\cdot{}_{e}^{a}\mathbf{T}\in\textbf{SE}(3)$$

(1)

where {o} is the world reference frame. The matrix ${}_{v}^{o}\mathbf{T}$ denotes the transformation of the virtual base frame {v} to the world frame, through two perpendicular prismatic joints and a revolute joint. $(x,y,\theta)$ represents the mobile base’s position and orientation in the 2-D plane. ${}_{b}^{v}\mathbf{T}$ represents a constant transformation of the mobile base {b} to the virtual base frame on the robot. ${}_{a}^{b}\mathbf{T}$ signifies a constant relative transformation from the base frame to the base of the manipulator arm {a}, and ${}_{e}^{a}\mathbf{T}$ represents the forward kinematics of the arm where the end-effector frame is {e}. This configuration is illustrated in Fig. 2.

The resulting expression could also be written as the following differential kinematics,

$\displaystyle{}^{0}\boldsymbol{\nu}_{e}$

$\displaystyle={}^{0}\mathbf{J}_{e}(x,y,\theta,\boldsymbol{q})\left(\begin{array}[]{c}\dot{\boldsymbol{q}}_{base}\\ \dot{\boldsymbol{q}}_{arm}\end{array}\right)$

(2)

For obstacle avoidance, building on previous works[13, 20], a minimum distance $d$ exists between a point $L$ on the robot’s link and a point $O$ on the obstacle in 3D space. The goal is to ensure that $d\geq S$, where $S$ represents a predefined safety threshold. If $d$ falls below $S$, a collision with the obstacle is considered to have occurred. Thus, the collision-free criterion is defined as follows:

$$\left\|\boldsymbol{p_{L}}(\boldsymbol{\theta}(t))-\boldsymbol{p_{O}}(t)\right\|_{2}\geq S$$

(3)

where $\boldsymbol{p_{L}}(\boldsymbol{\theta}(t))\in\mathbb{R}^{3}$ and $\boldsymbol{p_{O}}(t)\in\mathbb{R}^{3}$ represent the coordinates of point $L$ and point $O$, respectively. Evidently, inequality 3 is equivalent to

$$(\boldsymbol{p_{L}}(\boldsymbol{\theta}(t))-\boldsymbol{p_{O}}(t))^{\top}(\boldsymbol{p_{L}}(\boldsymbol{\theta}(t))-\boldsymbol{p_{O}}(t))\geq S^{2}$$

(4)

Building upon the model introduced by Jesse Haviland et al. [5, 13], we have enhanced the controller for mobile manipulation tasks by formulating it as a quadratic programming (QP) problem:

	$\displaystyle\min_{x}\quad f(\boldsymbol{x})$	$\displaystyle=\frac{1}{2}\boldsymbol{x}^{\top}\mathcal{Q}\boldsymbol{x}+\mathcal{C}^{\top}\boldsymbol{x}$		(5)
	$\displaystyle\text{ subject to }\quad\mathcal{J}\boldsymbol{x}$	$\displaystyle=\boldsymbol{\nu}_{e}-\boldsymbol{\delta}(t)$
	$\displaystyle G(\boldsymbol{x})$	$\displaystyle\leq\boldsymbol{r}_{I}$

where

	$\displaystyle G(\boldsymbol{x})$	$\displaystyle=\begin{bmatrix}-H(\boldsymbol{x})^{\top}H(\boldsymbol{x})\\ \boldsymbol{x}\\ -\boldsymbol{x}\end{bmatrix}\in\mathbb{R}^{k}$		(6)
	$\displaystyle\boldsymbol{r}_{I}$	$\displaystyle=\begin{bmatrix}-S^{2}\\ \mathcal{X}^{+}\\ -\mathcal{X}^{-}\end{bmatrix}\in\mathbb{R}^{k}$
	$\displaystyle H(\boldsymbol{x})$	$\displaystyle=(\boldsymbol{p_{L}}(\boldsymbol{\theta}(0)+\int_{0}^{t}\boldsymbol{x}dt)-\boldsymbol{p_{O}^{rel}}(t))$
	$\displaystyle\boldsymbol{p_{O}^{rel}}(t)$	$\displaystyle=\beta\psi\left({}^{o}_{v}\mathbf{T}^{-1}\left(x,y,\theta\right)\cdot{}_{obs}^{\ \ o}\mathbf{T}\right)$

$\boldsymbol{x}=\dot{\boldsymbol{\theta}}(t)^{\top}$ is the decision variable, $\boldsymbol{\delta}(t)$ represents the difference between the desired and actual end-effector velocity – relaxing the trajectory constraint. $\mathcal{Q}$ incorporates joint velocity between the mobile base and the robot arm, $\mathcal{C}$ incorporates manipulability maximisation and auxiliary performance tasks, $\mathcal{J}$ incorporates an augmented manipulator Jacobian. $\boldsymbol{p_{O}^{rel}}(t)$ represents the relative position of the obstacle point O to the virtual base. ${}_{obs}^{\ \ o}\mathbf{T}\in\textbf{SE}(3)$ is the obstacle in the virtual base frame. $\beta\in\mathbb{R}^{+}$ is a gain term, and $\psi(\cdot):\mathbb{R}^{4\times 4}\mapsto\mathbb{R}^{6}$ is a function which converts a homogeneous transformation matrix to a spatial twist.

For mobile manipulation, the accuracy and motion abilities of each joint exhibit variations. Generally, robot arms exhibit higher accuracy but have limited workspace and higher computational complexity. Conversely, while the mobile base extends the operational workspace significantly, it generally introduces a greater degree of motion inaccuracy compared to robot arms. Hence, we designed the Embodied Holistic Control(EHC) Function, as shown below:

	$\displaystyle\mathcal{Q}$	$\displaystyle=\left(\begin{array}[]{cc}\operatorname{diag}\left(\textbf{sig}(\omega)\right)&\mathbf{0}_{n_{base}\times n_{arm}}\\ \mathbf{0}_{n_{arm}\times n_{base}}&\mathbf{I}_{n_{arm}}\\ \end{array}\right)$		(7)
	$\displaystyle\mathcal{C}$	$\displaystyle=\left(\begin{array}[]{c}\mathbf{J}_{m}\\ \mathbf{0}_{6\times 1}\end{array}\right)$
	$\displaystyle\mathcal{Q}$	$\displaystyle\in\,\mathbb{R}^{(n+6)\times(n+6)},\ \mathcal{C}\in\,\mathbb{R}^{(n+6)}$

where

	$\displaystyle\omega$	$\displaystyle=\frac{\left\|{e}_{td}^{*}(\boldsymbol{\theta}(t),e(t))\right\|_{2}}{\left\|e(t)\right\|_{2}}$		(8)
	$\displaystyle{e}_{td}^{*}(\boldsymbol{\theta}(t),e(t))$	$\displaystyle=\mathop{\arg\min}_{p\in\boldsymbol{e}_{td}(\boldsymbol{\theta}(t))}\angle(p,e(t))$
	$\displaystyle\boldsymbol{e}_{td}(\boldsymbol{\theta}(t))$	$\displaystyle=\{p\in\textbf{SE}(3)|R(p,\boldsymbol{\theta}(t))>0.95\}$

$\textbf{sig}(\omega)$ dynamically balances the emphasis between the movement and manipulation, allowing the robot to switch its focus between efficient movement and precise manipulation as required by the task at hand. In particular, $e(t)\in\textbf{SE}(3)$ represents the end-effector’s pose relative to the target from perception in Fig. 2. $\boldsymbol{e}_{td}(\boldsymbol{\theta}(t))$ represents the set of end-effector poses with reachability greater than 95% for the given joint angles $\boldsymbol{\theta}(t)$, while ${e}_{td}^{*}(\boldsymbol{\theta}(t),e(t))$ denotes the element within this set that is most closely aligned with the direction of the current task error $e(t)$, serving as a threshold for motion emphasis under the current task objective. In addition, $R(\cdot)$ represents the reachability of the robot arm’s end-effector. The activation function $sig(\cdot)$ is incorporated into the robot control function, amplifying the emphasis on the specific motion. This enhancement improves the overall efficiency of motion planning. $\mathbf{J}_{m}$ is the manipulability Jacobian.

By employing this approach, the robot can achieve the EHC: When the robot is positioned at a considerable distance from the target object, the mobile base primarily handles the task of approaching the target. As the robot nears the target and within an embodied threshold $\omega$ accounting for the robot’s joint states $\boldsymbol{\theta}(t)$, the robot arm takes over as the primary actor in the manipulation process. Notably, this strategy enables the simultaneous and integrated planning of both locomotion and manipulation tasks, representing a significant advancement over traditional two-stage methods.

As illustrated in the Fig. 3(a), current Position-Based Servoing(PBS)[5] methods face the challenge of target loss during the reaching, which is particularly detrimental for reactive tasks. This is because, during the process of mobile manipulation, when the grasping orientation is not aligned with the forward-facing direction, such as when the orientation is downward or sideways, the end-effector of the robot arm changes its orientation prematurely during the reaching phase, resulting in a loss of visual contact with the target. Burgess et al.[21] propose improvements for grasping objects on the move. However, in their work, the object’s movement information is available to the robot. In real-world complex scenarios, the object’s positional information is only accessible to the robot through sensors. This necessitates that the robot should maintain the camera’s focus on the target during moving.

We utilize a controller that builds upon PBS and incorporates Monitoring-Position-Based Servoing (MPBS). This controller is designed to be parallel with the EHC function that dynamically balances the emphasis between monitoring and manipulation, based on $\textbf{sig}(\omega)$. The MPBS is formulated as Equation 9, according to Rodrigues’ theorem.

	$\displaystyle{}_{e}^{b}\mathbf{T}^{*}$	$\displaystyle=\textbf{sig}(\omega)\cdot{}^{b}_{e}\mathbf{T}+(1-\textbf{sig}(\omega))\cdot{}_{m}^{\ b}\mathbf{T}$		(9)
	$\displaystyle{}_{m}^{\ b}\mathbf{T}$	$\displaystyle=\begin{bmatrix}\boldsymbol{R_{d}}=e^{K\theta}&\boldsymbol{t_{e}}\\ \mathbf{0}&1\end{bmatrix}$
	$\displaystyle\mathbf{K}$	$\displaystyle=\left[\begin{array}[]{ccc}0&-k_{z}&k_{y}\\ k_{z}&0&-k_{x}\\ -k_{y}&k_{x}&0\end{array}\right]$

where the ${}_{e}^{b}\mathbf{T}^{*}\in\mathbf{SE}(3)$ is the end-effector target pose and ${}_{m}^{\ b}\mathbf{T}\in\mathbf{SE}(3)$ is the pose for monitoring. $\textbf{sig}(\omega)$ represents the threshold value for switching between the grasping pose and the monitoring pose, as shown in Fig. 1. $\mathbf{K}$ represents the rotation of axis $z$ towards the target, and $\theta$ represents the angle of rotation.

We utilize a RealMan compound robot to conduct experiments. This robot integrates a Water2 non-holonomic mobile base, featuring two drive wheels and four omni-directional wheels, enabling both in-place rotation and straightforward linear motion. The manipulator employed is an RM65-B six-degree-of-freedom robot arm. For visual perception, an Intel Realsense D435 camera is mounted at the end effector of the manipulator. The end effector gripper has a maximum aperture of 70mm and can exert a maximum gripping force of 1.5 kg. The computation is performed on a Alienware laptop, equipped with an Intel(R) Core(TM) i9-9900K CPU and a Nvidia GeForce RTX 2080 GPU.

To obtain a high-quality grasp pose, we enhanced GS-Net [22] that only considers the grasping problem of fixed arms in static scenes, and named the improved version GS-Net*.

GS-Net* improves mobile manipulation stability by generating grasp poses aligned with the object’s center of gravity and parallel to the object. After the approaching net phase, view scores are adjusted based on the angle to the center of gravity, with smaller angles receiving higher weights. Post-processing aligns the grasp poses with the object’s point cloud along the y-axis. Fig. 4 demonstrates that GS-Net* produces more stable, centered grasp poses. Compared with the original GS-Net, GS-Net* significantly increases the success rate by 28.9% for the same tasks.

Evaluation Metric.

• •

  Time for Grasping(s): In simulation, it refers to the average time taken to achieve the target pose. In real-world experiments, it represents the time taken to successfully grasp the target, considered only for successful trials.

• •

  Time for Monitoring(%): The ratio of the time the target object remains within the camera’s field of view to the total operation time, referred to as TfM.

• •

  Joint Velocity(rad/s for rotation, m/s for translation): For the base, which consists of two virtual joints, translation and rotation are considered, denoted as Base-T and Base-R, respectively. For the arm, it represents the combined velocity of the six rotational joints.

• •

  Success Rate(%): In the simulation, it refers to the percentage of trials in which the end-effector successfully reached the target pose. In real-world experiments, it refers to the percentage of trials in which the end-effector successfully grasps the object.

Baselines. Our improvements are primarily built on the NEO Controller[5, 13], thus, their approach serves as the baseline. The specific configurations are outlined below:

1. 1.

   TSMM: Traditional Two-Stage Mobile Manipulation. The robot first moves to a position $d$ away from the target object’s position, measured from the end-effector. Then the robot proceeds to grasp the object. In real-world experiments, $d$ is set to 0.2.

2. 2.

   NEO(e): The methodology of NEO[5], where the weight of the base is simply set to $\tfrac{1}{\left\|e(t)\right\|_{2}}$.

3. 3.

   NEO(c): To further demonstrate the validity of our work’s improvement direction, we adjust the weight of the base in the original method to a constant value $c$. This adjustment serves to validate the correctness of our method’s motivation.

Experimental Protocol. We conduct this performance test in the simulation. We introduce different levels of noise to various joints, as shown in Table I. In this task, the robot is asked to reach the random 50 points in sequence.

Results and Analysis. We compared NEO(c), NEO(e), and EHC. In each set of experiments, the three approaches individually reached the same 50 random points. We conducted a total of 10 sets of experiments, thereby each method approaching 500 random points. The results of the experiment are displayed in the Table II. We find that our approach aligns with the DMCG principle, prioritizing base movement for navigation at a distance and arm manipulation for grasping or approaching tasks at close range. Additionally, due to the uncertainty associated with randomly generated points, some target poses may be challenging to reach. Hence, we set a maximum time limit of 30 seconds for each approach attempt. Otherwise, it is considered as a failure. Our method’s ability to coordinate movements and manipulation enables the robot to reach the desired target pose in a shorter time, resulting in fewer failures.

Experimental Protocol. In the real-world experiment, the robot starts from a fixed location and sequentially grasp three objects of different distances on a table 1.5 metres away. These three objects were placed at the front, middle and back positions on the table, as shown in the Fig. 5. The time is the average time from the start of the robot’s movement, to the completion of the grasp, considering only successful trails.

Results and Analysis. We compare TSMM, NEO(c), NEO(e) and EHC. We conduct 15 sets of experiments, each grasping 45 objects. The results of the experiment are displayed in Table III, where NAN denotes that the value was not measured. Since, for the TSMM, measuring joint changes is meaningless as it includes two separate stages. Apart from the TSMM, we set a maximum grasping time of 30 seconds for each object. During real-world experiments, we find that the results in the real-world experiments align with those in simulations. EHC achieves shorter grasping times and higher success rates. we observe that the NEO(c) and NEO(e) still frequently perform major movements of the base even close to the target. Due to the limited precision of base movements, the robot faces difficulties in reaching the target smoothly, sometimes leading to failure. As for the TSMM, the robot frequently fails to grasp the most distant object because of the obstruction of the table. From Fig. 6, we can also visually observe that EHC enables smoother and faster mobile manipulation both in simulation and real-world experiments. Thus, EHC demonstrates excellent performance in real-world scenarios.

Experimental Protocol. In real-world experiment, the robot starts from a fixed location and grasps a single object positioned 1.5 meters away using different grasp poses, including forward, downward, and sideways orientations. The time is the average time cost for each grasping pose.

Results and Analysis. We compared NEO(without MPBS), EHC(without MPBS) and EHC(ours) as an ablation experiment. We conduct 15 sets of experiments, each involving 5 instances of forward, downward, and sideways grasping. The results of the experiment are displayed in the Table IV. A higher TfM value indicates that the robot is less likely to lose track of the target. We observe that, as to downward and sideways grasping orientations, without MPBS, the robot is more likely to lose track of the target in advance, as shown in Fig. 3(a). That is because the track of the target is not considered carefully as a constraint in the QP problem. In EHC(ours), MPBS ensures that the robot maintains monitoring on the target when the robot is at a distance.