Whole-Body Control for a Torque-Controlled Legged Mobile Manipulator

Abstract

The task of performing locomotion and manipulation simultaneously poses several scientific challenges, such as how to deal with the coupling effects between them and how to cope with unknown disturbances introduced by manipulation. This paper presents an inverse dynamics-based whole-body controller for a torque-controlled quadrupedal manipulator capable of performing locomotion while executing manipulation tasks. Unlike existing methods that deal with locomotion and manipulation separately, the proposed controller can handle them uniformly, which can take into account the coupling effects between the base, limbs and manipulated object. The controller tracks the desired task–space motion references based on a hierarchical optimization algorithm, given a set of hierarchies that define strict priorities and the importance of weighting each task within a hierarchy. The simulation results show the robot is able to follow multiple task–space motion reference trajectories with reasonable deviation, which proved the effectiveness of the proposed controller.

1. Introduction

In some dangerous environments, such as natural and man-made disasters, robots are more suitable than humans to execute tasks to protect emergency responders. Legged robots offer significant advantages over their wheeled or tracked counterparts, as these advantages assist in traversing challenging terrain and unstructured environments. To make these legged platforms more versatile and practical, equipping a legged robot with additional appendages, such as arms dedicated to manipulation tasks, expands the possibility of deploying them in the real world. A multi-limbed robotic system needs to make or break contact as well as modulate contact forces with its environment to move and balance its robot’s base (locomotion) and move the targeting object (manipulation). The governing dynamics in such complex robotic systems have a higher degree of freedom (DOF) and more complex hybrid transitions, making the design of controllers for such systems more challenging.

In the past few years, there has been a lot of progress in locomotion [1,2,3,4,5] and manipulation [6,7], but few solutions on the coordination of locomotion and manipulation are presented. Among these studies, the following works deserve attention. Some humanoid robots complete impressive locomotion and manipulation work [8,9] in the DARPA Robotics Challenge. However, the locomotion and manipulation tasks are not handled by the same controller, and the manipulation can only be performed when feet are planted and do not take any steps. The legged robot HyQ [10] equipped with a six-DOF arm HyArm performs a static walking gait while tracking motions of the arm. The presented controller integrates a mobile platform controller and a model-free arm controller combined with a payload estimation module. As a further study, a whole-body controller was developed for the quadrupedal robot HyQ equipped with a seven-DOF manipulator arm, which allows for the implementation of a Cartesian impedance control to coordinate tracking performance and desired compliance for the robot base and manipulator arm [11]. These two works show no demonstration of coordination between locomotion and manipulation. A hierarchical nonlinear control algorithm was developed, based on model predictive control, quadratic programming, and virtual constraints, to generate and stabilize motions [12]. Although some numerical simulation studies are presented to demonstrate the algorithm’s effectiveness, none of the manipulation tasks are achieved with the Kinova arm. Many impressive results have been achieved by Boston Dynamics’s robots BigDog [13], SpotMini and Spot. Robot Spot can simultaneously perform locomotion and manipulation tasks, e.g., opening and walking through a door with disturbances. However, no supplemental documentation illustrates methods and approaches that Boston Dynamics used to control these robots. The quadrupedal robot ANYmal [14] equipped with a six-DOF arm Jaco can perform dynamic locomotion while executing manipulation tasks, e.g., payload delivery, human-robot collaboration, and opening doors. Similar door opening scenarios have been achieved with a quadrupedal mobile manipulator, consisting of an ANYmal C platform equipped with a custom-made robotic arm [15]. Dynamic and kinematic consistent strategies are proposed to tackle the arising problems after equipping a quadruped robot with an arm [16]. The motion control of the arm is implemented separately; this decoupled control strategy is difficult to use to solve the problem of mutual interference between the arm and the base motions.

In order to fully explore the coupling effects between locomotion and manipulation, the dynamics of the entire system and the contact forces at all the robot’s limb end points need to be considered in the controller. This kind of controller, called whole-body controller (WBC), executes various tasks and accomplishes multiple control objectives. These controllers can be classified into inverse kinematics (IK)-based WBCs and inverse dynamics (ID)-based WBCs. The distinction between them is the output: IK-based WBCs produce joint velocities, while ID-based WBCs produce joint torques. Ultimately, the joint velocities/torques produced from both controllers are further transformed into motor currents with low-level impedance controllers or feedback torque controllers. Although those IK-based WBCs [7,17,18] are still used in robotic systems, the ID-based WBCs [19,20,21,22,23,24] are more suitable for torque-controlled robots. These controllers formulate the tracking problem within an optimization setting to resolve the task redundancy and incorporate equality and inequality constraints. An ID-based WBC has been used to control a legged manipulator [11], and it is formulated as a single quadratic program (QP) problem to find the ground reaction forces to track the desired Cartesian acceleration of task–space motion references. Although it can resolve the task redundancy via adjusting task weights, it is inevitable that tasks of higher importance will still be affected by tasks of lower importance. The hierarchical inverse dynamics approach proposed in [24] is extended for loco-manipulation in this work, for it can express complicated behaviours directly at the task level with strict enforcement of hierarchies between tasks. For example, it is useful to ensure that a balancing task of higher importance will take precedence over a motion tracking task of lower importance in case of conflicting goals.

This paper presents a whole-body controller, using hierarchical quadratic programming coupled with the full system dynamics, for a quadrupedal manipulator that creates adaptive behaviors and allows task–space and joint–space prioritized tasks to be executed. The controller tracks task–space motion references generated by a high-level planner described in [25]. The controller has been validated in simulation through a walking pivot task and a spring-loaded door opening task.

The remainder of the paper is structured as follows. The following section gives the full-body dynamics and its decomposition of the robotic system with floating-base. Section 3 presents the whole-body controller, formulating the control problem as a series of quadratic programs. Section 4 shows the simulation results of the proposed controller applied to the legged mobile manipulator Aliengo VX300 [26]. Finally, discussions and conclusions are given in Section 5.

2. Model Formulation

The model of a legged mobile manipulator can be formulated as a floating-base B to which limbs are attached. The position and orientation of the base frame $\left\{B\right\}$ with respect to the world frame $\left\{W\right\}$ are expressed as a three-dimensional vector ${}^W{\mathit{p}}_B\in {\mathbb{R}}^3$ and a Hamiltonian unit quaternion ${}^W{\mathit{q}}_B$ respectively. Limb joint positions are collected in the vector ${\mathit{q}}_j\in {\mathbb{R}}^{n_j}$, where the number of joints is $n_j=17$. The generalized coordinate vector $\mathit{q}$ and the generalized velocity vector $\mathit{v}$ are collected as

$$\mathit{q}=\left[\begin{array}{c}{}^W{\mathit{p}}_B\\ {}^W{\mathit{q}}_B\\ {\mathit{q}}_j\end{array}\right]\in SE\left(3\right)\times {\mathbb{R}}^{n_j},\mathit{v}=\left[\begin{array}{c}{}^W{\mathit{v}}_B\\ {}^W{\mathit{\omega }}_B\\ {\dot{\mathit{q}}}_j\end{array}\right]\in {\mathbb{R}}^{n_v},$$

(1)

where number of velocity vectors is $n_v=6+n_j$, and ${}^W{\mathit{v}}_B\in {\mathbb{R}}^3$ and ${}^W{\mathit{\omega }}_B\in {\mathbb{R}}^3$ are the linear and angular velocity of the floating-base with respect to the world frame expressed in the $\left\{W\right\}$ frame. The Aliengo VX300 robot shown in Figure 1 has $n_v=23$ DOFs, with six, twelve, and five DOFs describing the floating base, legs and the arm, respectively.

The equations of motion of a multi-limbed robot with a floating base can be written as

$$\mathit{M}\left(\mathit{q}\right)\dot{\mathit{v}}+\mathit{h}(\mathit{q},\mathit{v})={\mathit{S}}^T\mathit{\tau }+{\mathit{J}}_c^T\left(\mathit{q}\right)\mathit{\lambda },$$

(2)

where $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{n_v\times n_v}$ stands for the inertia matrix, and $\mathit{h}(\mathit{q},\mathit{v})$ is a vector of nonlinear terms (including Coriolis, centrifugal and gravity forces). The selection matrix $\mathit{S}=\left[\begin{array}{cc}{\mathit{0}}_{n_{\tau }\times (n_v-n_{\tau })}& {\mathit{I}}_{n_{\tau }\times n_{\tau }}\end{array}\right]$ represents the system under-actuation that the floating-base is not directly actuated by joint torques ${\mathit{\tau }}_j\in {\mathbb{R}}^{n_{\tau }}$. If all limb joints are actuated, then the number of actuated joints $n_{\tau }=n_j$. The vector of contact forces $\mathit{\lambda }$ is mapped to the joint–space torques through the support Jacobian ${\mathit{J}}_c\in {\mathbb{R}}^{3n_c\times n_v}$, which is obtained by stacking the end-effector Jacobians which relate generalized velocities to limb end motion as ${\mathit{J}}_c={\left[\begin{array}{ccc}{\mathit{J}}_{c_1}^T& \dots & {\mathit{J}}_{c_{n_c}}^T\end{array}\right]}^T$, with $n_c$ the number of limbs in contact.

For a robotic system with a floating base, its equations of motion Equation (2) can be decomposed as

$$\begin{array}{cc}\hfill {\mathit{M}}_u\left(\mathit{q}\right)\dot{\mathit{v}}+{\mathit{h}}_u(\mathit{q},\mathit{v})& ={\mathit{J}}_{c,u}^T\mathit{\lambda },\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill {\mathit{M}}_a\left(\mathit{q}\right)\dot{\mathit{v}}+{\mathit{h}}_a(\mathit{q},\mathit{v})& =\mathit{\tau }+{\mathit{J}}_{c,a}^T\mathit{\lambda },\hfill \end{array}$$

(4)

where Equation (3) is the first six equations of Equation (2) related to the floating base and Equation (4) is the rest of the equations. The former equation can be interpreted as the Newton–Euler equations of the whole robotic system, expressing the relationship between the change of momentum of the system and external contact forces.

For the Aliengo VX300 robot, the contact forces in point-feet and gripper are both modelled as three-dimensional linear contact forces, and the open-source rigid body dynamics Library Pinocchio [27,28] is used to compute its robot dynamics. The dynamic model used in this paper is based on the CAD model of the robot, which means that it is not very accurate as it does not take into account the contribution of any type of friction in the model. A good parameter identification of the dynamics will certainly make better performance [29,30]. Actually, good results with ID-based WBC can still be obtained without a perfect dynamic model since this approach is robust to model uncertainty [24].

3. Whole-Body Control

The whole-body controller formulates tasks as affine functions of generalized accelerations $\dot{\mathit{v}}$ and external generalized forces $\mathit{\lambda }$, and joint torques $\mathit{\tau }$ are then calculated from them. The control structure is depicted in Figure 2.

Thanks to the decomposition Equations (3) and (4), the torques $\mathit{\tau }$ only occur in the actuated part and are linearly dependent on generalized accelerations $\dot{\mathit{v}}$ and external forces $\mathit{\lambda }$ in the form

$$\mathit{\tau }={\mathit{M}}_a\left(\mathit{q}\right)\dot{\mathit{v}}+{\mathit{h}}_a(\mathit{q},\mathit{v})-{\mathit{J}}_{c,a}^T\mathit{\lambda }.$$

(5)

Since all occurrences of the joint torques $\mathit{\tau }$ can be replaced with Equation (5), the decision variable $\mathit{z}$ can be chosen as

$$\mathit{z}={\left[\begin{array}{cc}{\dot{\mathit{v}}}^T& {\mathit{\lambda }}^T\end{array}\right]}^T\in {\mathbb{R}}^{n_v+3n_c}.$$

(6)

All robot tasks are often modelled in the form of one general task $\mathit{T}$ defined as a set of linear equality and/or inequality constraints on the decision variable $\mathit{z}$:

$$\mathit{T}:\left\{\begin{array}{c}\mathit{A}\mathit{z}-\mathit{b}={\mathit{s}}_{eq}\hfill \\ \mathit{C}\mathit{z}-\mathit{d}⩽{\mathit{s}}_{in}\hfill \end{array}\right.,$$

(7)

where ${\mathit{s}}_{eq}\in {\mathbb{R}}^m$ and ${\mathit{s}}_{in}\in {\mathbb{R}}^k$ are slack variables. They are not predefined but part of the optimization variables. For a specific task, the above equality constraint and inequality constraint may exist at the same time, or only one of them exists.

Here are the robot tasks used in the ID based WBC designed for the Aliengo VX300 robot to execute locomotion and manipulation tasks:

(1) Dynamic consistency: To ensure physical consistency, the underactuated part of the equations of motion (3) can be written in the general task form (7) with

$$\begin{array}{cc}\hfill {\mathit{A}}_{dyn}& =\left[\begin{array}{cc}{\mathit{M}}_u\left(\mathit{q}\right)& {\mathit{J}}_{c,u}^T\end{array}\right],\hfill \\ \hfill {\mathit{b}}_{dyn}& ={\mathit{h}}_u(\mathit{q},\mathit{v}).\hfill \end{array}$$

(8)

which serves as a dynamic consistency equality constraint.

(2) Torque saturation limits: To ensure that the solution complies with the physical limitations, the joint torque limitation constraint can be formulated as

$$\begin{array}{cc}\hfill {\mathit{C}}_{tau}& =\left[\begin{array}{cc}-{\mathit{M}}_a\left(\mathit{q}\right)& {\mathit{J}}_{c,a}^T\\ {\mathit{M}}_a\left(\mathit{q}\right)& -{\mathit{J}}_{c,a}^T\end{array}\right],\hfill \\ \hfill {\mathit{d}}_{tau}& =\left[\begin{array}{c}{\mathit{h}}_a(\mathit{q},\mathit{v})-{\mathit{\tau }}^{min}\\ -{\mathit{h}}_a(\mathit{q},\dot{\mathit{q}})+{\mathit{\tau }}^{max}\end{array}\right].\hfill \end{array}$$

(9)

(3) Contact force limits: When the point feet are in contact with the ground, friction cone constraints on the resulting contact forces $\mathit{\lambda }\in {\mathbb{R}}^{3n_c}$ must be satisfied. The friction cone constraint means that the contact force of a single foot $\lambda ={\left[\begin{array}{ccc}{\lambda }_x& {\lambda }_y& {\lambda }_z\end{array}\right]}^T$ satisfies as follows:

$$\sqrt{{\lambda }_x^2+{\lambda }_y^2}⩽\mu {\lambda }_z,{\lambda }_z⩾0.$$

(10)

The above constraint Equation (10) can be approximated by

$$\left|{\lambda }_x\right|⩽\mu {\lambda }_z,\left|{\lambda }_y\right|⩽\mu {\lambda }_z,{\lambda }_z⩾0.$$

(11)

All contact force constraints can be obtained by collecting the Equation (11) constraints of active contacts together as follows:

$$\mathit{H}\mathit{\lambda }⩽\mathit{0},$$

(12)

so the contact force limits constraint can be written as

$$\begin{array}{cc}\hfill {\mathit{C}}_{cf}& =\left[\begin{array}{cc}\mathit{0}& \mathit{H}\end{array}\right],\hfill \\ \hfill {\mathit{d}}_{cf}& =\mathit{0}.\hfill \end{array}$$

(13)

(4) Contact motion task: Each contact introduces three constraint equations on its location ${\mathit{p}}_i\left(t\right)=const$, which can be differentiated twice to yield the following:

$$\begin{array}{cc}\hfill {\dot{\mathit{p}}}_i& ={\mathit{J}}_{c_i}\mathit{v}=\mathit{0},\hfill \\ \hfill {\ddot{\mathit{p}}}_i& ={\mathit{J}}_{c_i}\dot{\mathit{v}}+{\dot{\mathit{J}}}_{c_i}\mathit{v}=\mathit{0},\hfill \end{array}$$

(14)

where ${\mathit{J}}_{c_i}$ is the corresponding support Jacobian. By stacking the above constraints for all active contacts together, the contact motion task can be formulated as

$$\begin{array}{cc}\hfill {\mathit{A}}_{cm}& =\left[\begin{array}{cc}{\mathit{J}}_c& \mathit{0}\end{array}\right],\hfill \\ \hfill {\mathit{b}}_{cm}& =-{\dot{\mathit{J}}}_c\mathit{v}.\hfill \end{array}$$

(15)

(5) Cartesian space acceleration task: For the base, gripper and lifted feet, the relationship between its velocity in Cartesian space and the generalized velocity is

$$\dot{\mathit{x}}=\mathit{J}\mathit{v},$$

(16)

which can be differentiated as

$$\ddot{\mathit{x}}=\mathit{J}\dot{\mathit{v}}+\dot{\mathit{J}}\mathit{v}.$$

(17)

The deviation from the desired Cartesian acceleration can be penalized using

$$\begin{array}{cc}\hfill {\mathit{A}}_{cart}& =\left[\begin{array}{cc}\mathit{J}& \mathit{0}\end{array}\right],\hfill \\ \hfill {\mathit{b}}_{cart}& ={\ddot{\mathit{x}}}^{ref}+{\mathit{K}}_p^{cart}({\mathit{x}}^{ref}-\mathit{x})+{\mathit{K}}_d^{cart}({\dot{\mathit{x}}}^{ref}-\dot{\mathit{x}})-\dot{\mathit{J}}\mathit{v}.\hfill \end{array}$$

(18)

where ${\mathit{K}}_p^{cart}$ and ${\mathit{K}}_d^{cart}$ are diagonal positive definite matrices which define proportional and derivative gains, ${\mathit{x}}^{ref}$, ${\dot{\mathit{x}}}^{ref}$ and ${\ddot{\mathit{x}}}^{ref}$ specified by a high-level motion planner or the operator; $\mathit{x}$ and $\dot{\mathit{x}}$ are computed by forward kinematics based on the current robot state. Many tasks such as Base position control task, Base orientation control task, Gripper position control task, Gripper orientation control task and Swing foot position control task are specified with this form. It is worth noting that rotation-related operations should be handled based on requirements.

(6) Minimum motion task: To avoid internal drift, we can also introduce a joint–space task that spans the full configuration space of the robot. For example, we can penalize deviation from the nominal configuration with

$$\begin{array}{cc}\hfill {\mathit{A}}_{nom}& =\left[\begin{array}{cc}\left[\begin{array}{cc}\mathit{0}& \mathit{I}\end{array}\right]& \mathit{0}\end{array}\right],\hfill \\ \hfill {\mathit{b}}_{nom}& ={\mathit{K}}_p^{nom}\left({\mathit{q}}_{n_{\tau }}^{nom}-{\mathit{q}}_{n_{\tau }}\right)+{\mathit{K}}_d^{nom}\left({\mathit{v}}_{n_{\tau }}^{nom}-{\mathit{v}}_{n_{\tau }}\right).\hfill \end{array}$$

(19)

where ${\mathit{q}}_{n_{\tau }}^{nom}$ and ${\mathit{v}}_{n_{\tau }}^{nom}$ are the nominal generalized positions and velocities of actuated joints, and ${\mathit{K}}_p^{nom}$ and ${\mathit{K}}_d^{nom}$ are vectors which define proportional and derivative gains. It is worth mentioning that these tasks are mostly taken at the lowest priority.

Table 1. Task hierarchy (0 is the highest priority).

Priority	Task	Weight
0	Dynamic consistency	1
	Torque saturation limits	1
	Contact force limits	1
1	Contact motion task	1
2	Base position control task	1
	Base orientation control task	1
	Swing foot position control task	0.5
	Gripper position control task	1
	Gripper orientation control task	0.65
3	Minimum motion task	0.2

lists the prioritized tasks designed for the quadrupedal manipulator to execute locomotion and manipulation tasks. In general, the hierarchy prioritizes the stability of the robot itself, then performing movements. The highest priority satisfies dynamic constraints and hardware limitations. The second priority task enforces kinematic contact constraints. The third priority tasks are used to track task–space motion and force references. Finally, the lowest priority task allows the robot to keep a better-looking posture.

A set of tasks with priorities ${\mathit{T}}_1,{\mathit{T}}_2,\dots ,{\mathit{T}}_n$ can be solved in a strict prioritized way with a hierarchical optimization solver. To be specific, for the task with priority p, it is defined as:

$${\mathit{T}}_p:\left\{\begin{array}{c}{\mathit{A}}_p\mathit{z}-{\mathit{b}}_p={\mathit{s}}_{eq,p}\hfill \\ {\mathit{C}}_p\mathit{z}-{\mathit{d}}_p⩽{\mathit{s}}_{in,p}\hfill \end{array}\right..$$

(20)

The null space projector of this task is defined as:

$${\overline{\mathit{N}}}_p={\overline{\mathit{N}}}_{p-1}Null\left({\mathit{A}}_p{\overline{\mathit{N}}}_{p-1}\right).$$

(21)

Considering tasks ${\mathit{T}}_1,{\mathit{T}}_2,\dots ,{\mathit{T}}_{p+1}$, the augmented optional solution is

$${\overline{\mathit{z}}}_{p+1}^{*}={\overline{\mathit{N}}}_p{\mathit{z}}_{p+1}^{*}+{\overline{\mathit{z}}}_p^{*},p=1,\dots ,n-1,$$

(22)

where $\overline{{\mathit{N}}_1}={\mathit{N}}_1$ and ${\overline{\mathit{z}}}_1={\mathit{z}}_1$; ${\mathit{z}}_{p+1}^{*}$ is solved from the optimization problem:

$$\begin{array}{cc}\hfill {\mathit{z}}_{p+1}^{*},{\mathit{s}}_{in,p+1}^{*}=\underset{\mathit{z},{\mathit{s}}_{in}}{min}& \frac{1}{2}\parallel {\mathit{A}}_{p+1}({\overline{\mathit{N}}}_p\mathit{z}+{\overline{\mathit{z}}}_p^{*})-{\mathit{b}}_{p+1}{\parallel }^2+\frac{1}{2}{\parallel {\mathit{s}}_{in}\parallel }^2\hfill \\ \hfill s.t.& {\mathit{C}}_{p+1}({\overline{\mathit{N}}}_p\mathit{z}+{\overline{\mathit{z}}}_p^{*})-{\mathit{d}}_{p+1}⩽{\mathit{s}}_{in}\hfill \\ \hfill & {\mathit{C}}_p({\overline{\mathit{N}}}_p\mathit{z}+{\overline{\mathit{z}}}_p^{*})-{\mathit{d}}_p⩽{\mathit{s}}_{in,p}^{*}\hfill \\ \hfill & \dots \hfill \\ \hfill & {\mathit{C}}_2({\overline{\mathit{N}}}_p\mathit{z}+{\overline{\mathit{z}}}_p^{*})-{\mathit{d}}_2⩽{\mathit{s}}_{in,2}^{*}\hfill \\ \hfill & {\mathit{C}}_1({\overline{\mathit{N}}}_p\mathit{z}+{\overline{\mathit{z}}}_p^{*})-{\mathit{d}}_1⩽{\mathit{s}}_{in,1}^{*}\hfill \end{array}.$$

(23)

After solving a sequential quadratic program with Equation (23) and obtaining the optimal generalized accelerations ${\mathit{v}}^{*}$ and external forces ${\mathit{\lambda }}^{*}$ with Equation (22), the optimal joint torques ${\mathit{\tau }}^{*}$ can be computed with Equation (5).

4. Simulation Studies

The simulation was conducted in PyBullet [31], a Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics software development kit (SDK). The dynamics library Pinocchio was employed to update robot dynamic terms such as inertia matrix and Jacobians. The open-source OSQP solver [32] and its C++ wrapper OSQP-Eigen was used to solve all the QPs in the proposed controller running at 500 Hz.

4.1. Walking Pivot

The walking pivot simulation experiment tests the whole-body controller by commanding the gripper to stay at a fixed position in the world frame. At the same time, the legged robot walks around the gripper, as shown in Figure 3. In detail, the robot stands in place (0∼1 s), steps back from the initial position (1∼3 s), crouches down (3∼4 s) and stands up (4∼5 s), rotates 360 degrees around a fixed point (5∼21 s), walks forward to the initial position (21∼23 s) and then stands still.

The translation and rotation variation of the base are shown in Figure 4 and Figure 5, respectively. The base position was able to follow the reference with less than 0.01 m deviation in x and y direction. From the graph, there is a 0.03 m deviation from the reference in z direction. The reason is that during the crouches down movement, the task of keeping the gripper position has the same weight as the movement of the base. Therefore, the base sacrifices vertical movement to maintain gripper height. The base orientation reference can be followed well with less than 0.015 rad deviation in roll angle and 0.003 rad in pitch angle. The rotation around a fixed point was divided into four steps to test the ability of acceleration and deceleration of the proposed method. Therefore, from Figure 5, the angular velocity in the yaw direction oscillated four times and was reflected in the orientation trajectory.

The translation and rotation variation of the gripper are shown in Figure 6 and Figure 7, respectively. From Figure 6, the maximum deviation in x, y and z directions is 0.05 m. the Gripper position control task has the same weight with the Base position control task; thus, the results share similar characteristics with the results of the base. On the other hand, due to a lower weight in gripper orientation and the design of the arm, the gripper’s angular velocity in the yaw direction has larger deviation than base. A top-down view can have a more straightforward demonstration of the data mentioned above, as shown in Figure 8.

4.2. Spring-Loaded Door Opening

To illustrate the ability of the whole-body controller to perform locomotion and manipulation tasks simultaneously in real-world missions, we demonstrate the execution of opening a spring-loaded door while performing a trotting gait. As shown in Figure 9, the door opening task can be broken down into the following processes: approach the door (1∼5 s), locate the door handle (5∼7 s), open the gripper (7∼7.5 s), approach the handle (7.5∼8 s), clamp on to the handle (8∼8.5 s), turn the handle (8.5∼10.5 s), gently push the door open (10.5∼12.5 s), reposition the handle (12.5∼13 s), hold the door while walking pass (13∼18 s), rotate the base while keeping the gripper position fixed (18∼21 s) and finally reverse to release the spring-loaded door (21∼25 s).

Figure 10 shows the top view of the position and orientation trajectories of the base, gripper and door during the door opening task. As can be seen from the picture, with unknown forces from the spring-loaded door acting on the gripper, the robot can still finish the complex door opening task.

Figure 11 shows the base velocity and base height. The maximum velocity deviation of 0.06 m/s appears in y direction around 3 s, and the maximum height deviation of 0.015 m appears near 20 s due to a higher weight in the Base position control task. The developed whole-body controller prioritizes all robot tasks, which is beneficial for tracking reference after keeping balance.

Figure 12 shows the desired torques of arm joints generated by the whole-body controller during the door opening process. The elbow joint fluctuates intensively from 1 s to 5 s due to shoulder and elbow joints parallel to each other and having more than one kinematic solution for a gripper pose relative to the base. In addition, the base velocity contributes to the fluctuation. It can also be seen from the graphic that the joint torques of the arms change considerably to open the door when the robotic arm interacts with the spring-loaded door.

Figure 13 shows the torque of each joint of the door. It should be noted that, in simulation, a joint controller was added to each joint of the door to function as a door closing device and handle spring. It can be seen from these two graphics that when the robot starts to interact with the door, the joint torques of the arm have significant increase. This change indicates that the whole-body controller adjusts the desired torque value to balance the unknown external force and ensure that the reference base pose and gripper pose can be tracked. At the same time, the door moves passively, and resistance torque is generated in the corresponding joint of the door.

Figure 14 presents the force acting from the ground to the feet in the world frame. Since the robot is walking in trot gait, the force acting on the front left foot is similar to the rear right foot, and the front right foot is similar to the rear left foot. Therefore, we only choose one pair to show the result. During the process of approaching the door, the force in the x direction reflects the acceleration and deceleration of the robot in the same direction. The force in the y direction exists to balance the robot and keep a straight walking trajectory. The force generated by interacting with the door is relatively small compared to the force created by the trotting motion, and can be distinguished in the graph. The maximum force in the z direction when the robot trots is twice the maximum force when it stands still. This is because the robot lifts diagonal legs simultaneously at a higher speed, and only two legs make contact with the ground when it trots.

5. Conclusions

A whole-body controller is implemented which uses a hierarchical inverse dynamics approach considering the dynamics of the whole robotic system, enabling a torque-controlled quadrupedal manipulator capable of performing locomotion and manipulation tasks simultaneously. Unlike existing methods that separate the whole control problem into locomotion and manipulation sub-problems, the controller can uniformly balance locomotion and manipulation, which makes it possible to account for the coupling effects between the base, limbs and object. In addition, we found that the controller enables the robot to passively respond external disturbances and complete the tracking task even in the absence of an accurate root dynamics model and the dynamic properties of the manipulated object are unknown. The above advantages are very beneficial to deploy legged manipulators in unstructured and uncertain environments to complete tasks. The effectiveness of the controller was thoroughly demonstrated in two specially designed cases based on real-life scenarios, which include walking and manipulating objects.

The current robot arm has only five joints, with two of them parallel to each other, which limits the movement of the gripper in space. Further study will employ a better arm to improve the system performance. Future work will also focus on developing a whole-body motion planner that plans whole-body motion and force trajectories tracked by the controller for tasks combining dynamic locomotion and manipulation.