Current numerical methods for solving kinematic identification (KI) and inverse kinematics (IK) are limited in accuracy, convergence rates, and robustness, necessitating further enhancement. This paper presents a modified Halley method for solving the KI and IK problems of serial robots based on the product of exponentials formula, achieving quintic convergence. Specifically, a general error model is first established based on exponential coordinates, and KI and IK are reformulated as root-finding problems. Next, the modified Halley method, which we prove to be a fifth-order method and incorporates a damping strategy, is proposed to resolve the singularity issue and enhance robustness. Subsequently, the Jacobian and Hessian matrices required for the proposed method are analytically derived based on the time differential of exponentials. Furthermore, highly simplified explicit formulas for these matrices are presented for the IK problem. Simulations on serial robots with various configurations validate the proposed method's accuracy, convergence rates, and robustness in solving KI and IK problems, as well as its advantages over the state-of-the-art. Additionally, experimental validation of KI on two physical robots further demonstrates the effectiveness of the proposed method. Our custom-written MATLAB and C++ codebases are made publicly available for download.

Recently, the research on mobile manipulators has attracted increasing attention. Ensuring that mobile manipulators can meet obstacle avoidance constraints and efficiently accomplish assigned tasks in dynamic environments remains a significant challenge. To address this issue, this paper proposes an integrated framework for environment perception, real-time planning, and control optimization. Firstly, we develop a fusion map that combines euclidean signed distance field (ESDF) with clustered point clouds occupying cubes, enabling robots to perceive more precise environmental information in complex and changing conditions. Secondly, we introduce a novel rapid generation strategy for 6-DOF guide point sequences, which directs the mobile manipulator to follow the most efficient path to the target location while making real-time adjustments to avoid dynamic obstacles. Additionally, utilizing optimized nonlinear model predictive control (NMPC), we design a whole-body motion controller for the mobile manipulator to prevent the system from becoming trapped in local optima, thereby allowing the manipulator to adjust its state tracking guide points promptly in complex indoor environments. Finally, the proposed algorithm was implemented on a mobile manipulator with an Ackerman base and tested through both simulations and real-world experiments.

The attention given to impedance control in recent years does not match a similar focus on the choice of impedance values that the controller should execute. Current methods are hardly general and often compute fixed controller gains relying on the use of expensive sensors. In this article, we address the problem of online impedance planning for Cartesian impedance controllers that do not assign the closed-loop inertia. We propose an optimization-based algorithm that, given the Cartesian inertia, computes the stiffness and damping gains without relying on force/torque measurements and so that the effects of perturbations are less than a maximum acceptable value. By doing so, we increase robot resilience to unexpected external disturbances while guaranteeing performance and robustness. The algorithm provides an analytical solution in the case of impedance-controlled robots with diagonally dominant inertia matrix. Instead, established numerical methods are employed to deal with the more common case of nondiagonally dominant inertia. Our work attempts to create a general impedance planning framework, which needs no additional hardware and is easily applicable to any robotic system. Through experiments on real robots, including a quadruped and a robotic arm, our method is shown to be employable in real time and to lead to satisfactory behaviors.

Legged robot locomotion requires the planning of stable reference trajectories, especially while traversing uneven terrain. The proposed trajectory optimization framework is capable of generating dynamically stable base and footstep trajectories for multiple steps. The locomotion task can be defined with contact locations, base motion or both, making the algorithm suitable for multiple scenarios (e.g., presence of moving obstacles). The planner uses a simplified momentum-based task space model for the robot dynamics, allowing computation times that are fast enough for online replanning. This fast planning capability also enables the quadruped to accommodate for drift and environmental changes. The algorithm is tested on simulation and a real robot across multiple scenarios, which includes uneven terrain, stairs and moving obstacles. The results show that the planner is capable of generating stable trajectories in the real robot even when a box of 15 cm height is placed in front of its path at the last moment.

This paper presents a control framework to teleoperate a quadruped robot's foot for operator-guided haptic exploration of the environment. Since one leg of a quadruped robot typically only has 3 actuated degrees of freedom (DoFs), the torso is employed to assist foot posture control via a hierarchical whole-body controller. The foot and torso postures are controlled by two analytical Cartesian impedance controllers cascaded by a null space projector. The contact forces acting on supporting feet are optimized by quadratic programming (QP). The foot's Cartesian impedance controller may also estimate contact forces from trajectory tracking errors, and relay the force-feedback to the operator. A 7D haptic joystick, Sigma.7, transmits motion commands to the quadruped robot ANYmal, and renders the force feedback. Furthermore, the joystick's motion is bounded by mapping the foot's feasible force polytope constrained by the friction cones and torque limits in order to prevent the operator from driving the robot to slipping or falling over. Experimental results demonstrate the efficiency of the proposed framework.

An important issue when synthesizing legged locomotion plans is the combinatorial complexity that arises from gait pattern selection. Though it can be defined manually, the gait pattern plays an important role in the feasibility and optimality of a motion with respect to a task. Replacing human intuition with an automatic and efficient approach for gait pattern selection would allow for more autonomous robots, responsive to task and environment changes. To this end, we propose the idea of building a map from task to gait pattern selection for given environment and performance objective. Indeed, we show that for a 2D half-cheetah model and a quadruped robot, a direct mapping between a given task and an optimal gait pattern can be established. We use supervised learning to capture the structure of this map in a form of gait regions. Furthermore, we propose to construct a warm-starting trajectory for each gait region. We empirically show that these warm-starting trajectories improve the convergence speed of our trajectory optimization problem up to 60 times when compared with random initial guesses. Finally, we conduct experimental trials on the ANYmal robot to validate our method.

Legged robots have great potential to perform complex loco-manipulation tasks, yet it is challenging to keep the robot balanced while it interacts with the environment. In this paper we investigated the use of additional contact points for maximising the robustness of loco-manipulation motions. Specifically, body-ground contact was studied for its ability to enhance robustness and manipulation capabilities of quadrupedal robots. We proposed equipping the robot with prongs: small legs rigidly attached to the body which create body-ground contact at controllable point-contacts. The effect of these prongs on robustness was quantified by computing the Smallest Unrejectable Force (SUF), a measure of robustness related to Feasible Wrench Polytopes. We applied the SUF to evaluate the robustness of the system, and proposed an effective approximation of the SUF that can be computed at near-real-time speed. We developed a hierarchical quadratic programming based whole-body controller that can control stable interaction when the prongs are in contact with the ground. This novel prong concept and complementary control framework were implemented on hardware to validate their effectiveness by showing increased robustness and newly enabled loco-manipulation tasks, such as obstacle clearance and manipulation of a large object.

In this paper, we propose a method for selecting the optimal footholds for legged systems. The goal of the proposed method is to find the best foothold for the swing leg on a local elevation map. First, we evaluate the geometrical characteristics of each cell on the elevation map, checks kinematic constraints and collisions. Then, we apply the Convolutional Neural Network to learn the relationship between the local elevation map and the quality of potential footholds. During execution time, the controller obtains the qualitative measurement of each potential foothold from the neural model. This method evaluates hundreds of potential footholds and checks multiple constraints in a single step which takes 10 ms on a standard computer without GPU. The experiments were carried out on a quadruped robot walking over rough terrain in both simulation and real robotic platforms.

Real world applications require robots to operate in unstructured environments. This kind of scenarios may lead to unexpected environmental contacts or undesired interactions, which may harm people or impair the robot. Adjusting the behavior of the system through impedance control techniques is an effective solution to these problems. However, selecting an adequate impedance is not a straightforward process. Normally, robot users manually tune the controller gains with trial and error methods. This approach is generally slow and requires practice. Moreover, complex tasks may require different impedance during different phases of the task. This paper introduces an optimization algorithm for online planning of the Cartesian robot impedance to adapt to changes in the task, robot configuration, expected disturbances, external environment and desired performance, without employing any direct force measurements. We provide an analytical solution leveraging the mass-spring-damper behavior that is conferred to the robot body by the Cartesian impedance controller. Stability during gains variation is also guaranteed. The effectiveness of the method is experimentally validated on the quadrupedal robot ANYmal. The variable impedance helps the robot to tackle challenging scenarios like walking on rough terrain and colliding with an obstacle.

We consider a virtual manipulator in grasping scenarios which allows us to capture the effect of the object dynamics. This modeling approach turns a multi-arm robot into an underactuated system. We observe that controlling floating-base multi-leg robots is fundamentally similar. The Projected Inverse Dynamics Control approach is employed for decoupling contact consistent motion generation and controlling contact wrenches. The proposed framework for underactuated robots has been evaluated on an enormous robot hand composed of four KUKA LWR IV+ representing fingers cooperatively manipulating a 9kg box with total 28 actuated DOF and six virtual DOF representing the object as additional free-floating robot link. Finally, we validate the same approach on ANYmal, a floating-base quadruped with 12 actuated DOF. Experiments are performed both in simulation and real world.

Legged robots have many potential applications in real-world scenarios where the tasks are too dangerous for humans, and compliance is needed to protect the system against external disturbances and impacts. In this paper, we propose a model-based controller for hierarchical tasks of legged systems subject to external disturbance. The control framework is based on projected inverse dynamics controller, such that the control law is decomposed into two orthogonal subspaces, i.e., the constrained and the unconstrained subspaces. The unconstrained component controls multiple desired tasks with impedance responses. The constrained space controller maintains the contact subject to unknown external disturbances, without the use of any force/torque sensing at the contact points. By explicitly modelling the external force, our controller is robust to external disturbances and errors arising from incorrect dynamic model information. The main contributions of this paper include (1) incorporating an impedance controller to control external disturbances and allow impedance shaping to adjust the behaviour of the motion under external disturbances, (2) optimising contact forces within the constrained subspace that also takes into account the external disturbances without using force/torque sensors at the contact locations. The techniques are evaluated on the ANYmal quadruped platform under a variety of scenarios.