{'fraction_non_alphanumeric': 0.028625078239098687, 'fraction_numerical': 0.008721051533486334, 'mean_word_length': 4.537430683918669, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 3, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Real-time Trajectory Optimization and Control for Ball Bumping with Quadruped Robots

This paper studies real-time motion planning and control for ball bumping motion with quadruped robots. To enable the quadruped to bump the flying ball with different initializations, we develop a nonlinear trajectory optimization-based planning scheme that jointly identifies the take-off time and state to achieve accurate ball hitting during the flight phase. Such a planning scheme employs a two-dimensional single rigid body model that achieves a satisfactory balance between accuracy and efficiency for the highly time-sensitive task. To precisely execute the planned motion, the tracking controller needs to incorporate the strict time-state constraint imposed on the take-off and ball-hitting events. To this end, we develop an improved model predictive controller that respects the critical time-state constraints. The proposed planning and control framework is validated with a real Aliengo robot. Experiments show that the problem planning approach can be computed in approximately 60ms on average, enabling successful accomplishment of the ball bumping motion with various initializations in real time.


I. INTRODUCTION
Jumping and interacting with objects in the air is one of the most amazing behaviors that animals can perform. Quadrupedal animals like leopards can leap and catch birds; dogs can jump in the air and hit the ball using their heads. Usually, this type of acrobatic behavior consists of multiple phases, including jumping, interacting with the target object, and landing phases. Ball bumping motion, which requires the quadruped to jump into the air and hit a falling ball toward a goal location, is one of the most representative acrobatic motions for quadrupeds. Different from standard quadrupedal locomotion on the ground, such a ball bumping motion is highly time-sensitive, i.e., if the juggler does not arrive at the expected position with the specific velocity at a proper time, then the ball can not reach the desired region. In this paper, we aim to tackle the specific ball bumping problem with quadruped robots, which serves as a starting point for exploring more highly dynamic motion planning and control frameworks for future robotic applications.

A. Related Works
Dynamic locomotion for quadruped robots has attracted a considerable amount of research attention during the past decade. Thanks to the rapid advancements of both hardware All  and algorithms, quadrupedal robots have demonstrated impressive locomotion skills on flat and uneven terrains [1]- [4].
Fundamentally speaking, achieving jumping motion for quadrupeds can be formulated as trajectory optimization problems in general [5]. MIT Cheetah 3 is capable of jumping onto a desk with a height of 30 inches by an offline trajectory optimization that considers full-body kino-dynamics in a 2D vertical plane [6]. Chigonoli et al. [7] proposed a hierarchical planning framework with centroidal dynamics and joint-level kinematics to achieve Omnidirectional jumping, which takes on average 0.55 s to find a reference trajectory plan. Nguyen et al. [8] synthesized a full-body trajectory optimization to achieve 3D jump with quadrupeds, which takes several minutes to solve. Among the pioneering works trying to plan and control jumping motions for quadrupeds, Park et al. [9], [10] adopted a 2D single rigid body model for quadrupedal dynamics and developed an event-triggered jumping controller that accomplishes jumping over obstacles on MIT Cheetah 2 quadruped. Li et al. [11] studied the jumping motion of quadrupeds with only two rear legs and developed a hierarchical planning and control framework that can be implemented in real-time based on a springloaded inverted pendulum model [12], which is verified with simulations. As the reversed process of jumping, landing control with quadrupeds has also been considered. Jeon et al. [13] developed a supervised learning-based warm start interface for nonlinear landing trajectory planning to improve the performance of quadrupedal landing. How to exploit the conservation of angular momentum to help modulate the robot's configuration during the flight phase has also attracted recent research attentions [14], [15], which further improves the landing performance of quadrupeds. Despite these amazing demonstrations of jumping motion control for quadrupeds, problems studying quadruped jumping with physical interactions with other objects during the flight phase have not been studied adequately in the literature.
More recently, the problem of operating quadrupeds to physically interact with other objects to achieve more complicated tasks has attracted more research attention. Ji et al. [16] considered the soccer shooting problem with a quadruped robot and tackled the problem via a hierarchical reinforcement learning strategy. Shi et al. [17] adopted a learning-based approach to enable a quadruped robot to manipulate balls with its legs. All these early investigations focus on scenarios where the target object is static or quasi-static. How to design effective planning and control strategies for interaction with moving objects remains lacking. In addition to quadrupeds, acrobatic motions for bipedal robots have also been studied extensively in the literature. In particular, investigations on jumping control with bipeds date back to Raibert's seminal works [18], [19]. Hierarchical planning and control frameworks have been long employed in the synthesis of acrobatic bipedal motions. Wensing et al. [20] proposed a conic optimization-based task-space control to achieve motions like ball kicking and standing broad jump with a humanoid robot. Xiong et al. [21] developed an optimization-based controller for biped hopping with biped Cassie. Chigonoli et al. [22] employed the trajectory optimization-based approach for synthesizing dynamic acrobatic motions with a humanoid robot. Reinforcement learning-based approaches have also demonstrated impressive performance in achieving agile biped behaviors [23], [24]. Poggensee et al. [25] implemented ball-juggling on biped Cassie, the motion is a periodic orbit, and there is no flying phase while bounding.

B. Contributions
The main contributions of this work are summarized as follows. First, to incorporate the challenging time-state constraint in the ball hitting problem, we develop a realtime optimization scheme to find the critical time-state pair based on a two-dimensional single rigid body model. Such an online planning scheme allows us to find the appropriate take-off state and time to achieve precise ball hitting, which addresses the main challenge in accomplishing the ball bumping problem. Second, to enable the quadruped to accurately execute the planned motion, we develop an improved model predictive controller that actively adjusts the ground reaction forces based on the time remaining for the jumping motion. Such a modification effectively guarantees the execution of the time-restricted motion, making the ball bumping motion practically achievable. Third, we demonstrate the effectiveness and performance of the proposed framework on a real quadruped platform, successfully accomplishing the ball bumping motion with various initializations.

II. PROBLEM STATEMENT
Our goal is to control the quadruped to bump the falling ball to the desired landing point. Throughout this paper, we assume the initial state of the ball is given, including the released position, velocity, and the released time t s . As depicted in Fig. 2, the whole process can be split into three phases. The first phase is takeoff, in which the quadruped starts to jump at t j according to the released time t s of the ball, then executes the online planned jumping motion and finally takes off at t t . The second is a flying phase, and the quadruped will fly off the ground based on the takeoff velocity, then collide with the ball at t h in order to let the ball reach the desired landing region. The last one is the landing phase, where the quadruped will keep balance after contact with the ground. The ball bumping motion studied in this paper requires an active real-time plan of a feasible trajectory for the quadruped in response to the falling ball's state and accurate execution of the planned trajectory. Such a problem calls for a proper balance between the efficiency and accuracy of the proposed approach. In this section, we provide our modeling of the quadruped-ball system used for planning the ball bumping task and then give an overview of the proposed framework.
A. Modeling of Jumping Quadruped and the Falling Ball 1) quadruped model: In order to simplify the planning of jumping motion while reserving acceptable accuracy, we consider a 2D planar single rigid body model(2D-SRBM) for quadruped in the sagittal plane. The state is the position of the center of mass(COM) and the pitch angle of the quadruped, denoted as x q ∈ SE(2). The dynamics are given as follows d dt where [a] × is defined as an operator such that [a] × b = a 1 b 2 − a 2 b 1 for all a, b ∈ R 2 , 0 n and 0 m×n are the n by n and m by n zeros matrices respectively. I ∈ R 2×2 is the inertial matrix in x-z plane. As shown in Fig. 3, r f , r r are the vectors from the COM to the front and rear feet respectively. f f , f r are the corresponding ground reaction forces. 2) flying ball/quadruped model: Because of the lightweight legs and the short duration of the flying phase, it is hard to adjust the angular momentum of the torso in the air. Therefore, we can consider the position trajectory of the quadruped torso between time t t and t h as a ballistic trajectory. As for the rotation of the quadruped, we believe the robot will keep a constant angular velocity after taking off.
As for the falling ball, if the chosen ball has high-density mass and the velocity is low, then we can neglect the aerodynamic effect. Hence, the motion of the falling ball can also be modeled as a ballistic trajectory before and after the collision.
3) quadruped-ball collision model: The collision model is used to dictate the velocity-changing rules for both the ball and quadruped while the collision occurs. As shown in Fig. 3, we assume the collision will not affect the motion along the tangential direction of the contact surface. According to the conservation of momentum and considering the enormous difference between the mass of the ball and the quadruped, we assume that the quadruped's state will not change after the collision, while the velocity of the ball after the collision is determined as followṡ where t h− and t h+ are the time instants before and after the collision, R h ∈ R 2×2 represents the orientation from the body to the world frame at t h , e represents the coefficient of restitution, which is set as 0.8 in our implementation, v q is the contact point's velocity of the robot, which is given by where 1 n is the n × n identity matrix, l h is the distance between the collision point and COM in the tangential direction of the torso, and h body is the body height.

B. Overview of the Proposed Framework
The whole ball bumping motion is achieved through four modules: jumping motion planning, jumping controller, flying controller, and landing controller. Because of the agility of the jumping motion, a nonlinear program is formulated to get a dynamically feasible trajectory. The details of the formulation will be illustrated in III. Then the model predictive control is used for the jumping tracking controller. Finally, when foot contact is detected, the system will use the landing controller to help the quadruped to keep balance. An overview of this framework is shown in Fig. 4.

III. REAL-TIME PLANNING OF BALL BUMPING
The key point of this ball bounding problem is whether the quadruped can reach the desired velocity when it takes off according to the ball's desired landing point. Also, the robot needs to react quickly after detecting the ball, so the planning time needs to be as short as possible. A straightforward way of real-time implementation is to find a pre-collision state of the quadruped leading the post-collision ball to fly along its ballistic trajectory into the desired landing point with the minimum takeoff twist criterion. However, if only given the take-off state and time by the ball's landing point, because of the high-resolution requirement of jumping motion, it is hard for a controller to reach the desired state accurately.
Therefore, we also need to consider how the robot can reach the desired takeoff state. Some works used 3D single rigid body model as the dynamics of the quadruped for the motion planning, but considering the real-time requirement of the algorithm, it can not satisfy our requirement. Because of the specificity of our jumping motion, which is bilaterally symmetrical, we choose the 2D single rigid body model instead. To further simplify the problem to accelerate the computation, we also set the jumping start time t j manually according to the ball released time to guarantee the planning can be computed before the robot begins to move.
Finally, in order to find one possible take-off state configuration and a dynamically feasible trajectory in real time, we formulate the following nonlinear optimization problem ] T ∈ R 6 is the state and input of the 2D-SRBM respectively, and x[·], u[·] is the state and input trajectories between time t j and t t . Here t = [t t , t h ] T is the time frames which need to be optimized, p = [l h , θ h ] T is the collision configuration waited to be optimized. There are totally N timesteps for this jumping motion, ω i is the cost are the functions for the discrete 2D-SRBM, the ball's landing position, and the friction cone, where δt = tt−tj N −1 is the timestep for dynamics. And the collision position and takeoff state is bounded by l,l and x,x respectively.

A. Ball's Landing Point Design
From the above formulation, we optimized the collision time t h and collision stateẋ q (t h ) at the same time. And they are associated by the desired landing point of the ball.
After the collision, the ball will fall only under the effect of gravity as described at II-A.2. With the velocityẋ b (t h+ ) and position x b (t h+ ) of the ball after the collision, we can get the landing point of the ball, which is what we need. With z axis value of the velocityẋ b,z (t h+ ) and position x b,z (t h+ ) of the ball after the collision, we can easily get the time t e when the ball touch the desired ground, the height of the ground is denoted as x b,z (t e ), then t e can be computed Then using the dynamics of the ball (equation (2)), we can get the position function for the ball's landing point Then because during the planning, we only consider the trajectory from time t j to t t , so here we need to induce x q (t h ) by x q (t t ) using the model described at II-A.2 where ∆t = t h − t t , g q = [0 g 0] T is the gravity.
Here we formulate the ball's landing point task as a constraint not cost, the main reason is that the jumping task is time-sensitive, so it's better to add this task into the constraint to guarantee the reference trajectory can lead the quadruped to reach the desired takeoff state accurately at the desired time instance. Besides, as for the computational cost and success rate, we verified that the success rate of putting the task into the constraint or the cost was roughly the same, and they all spent nearly 60 ms to 80ms.

B. Optimization Objective Design
Because we only need to get a feasible solution, so we only choose the input norm u[k] T u[k] as the optimization cost, which means if the problem has a solution, whatever it is globally optimal, it can be used for our jumping motion.
IV. MULTI-PHASE CONTROLLER DESIGN FOR BALL BUMPING As shown in Fig. 4, there are three main phases in this ball bumping task, in order to achieve the desired landing point accurately and keep the whole process safe, we customize the controllers for all these three phases respectively.

A. Jumping Controller
The jumping motion is the most important part of the ball bumping task, and its focus is different from other classical locomotion tasks of quadrupeds, like walking on narrow terrains. Because jumping motion changes the velocity in small duration, which means it is a high-resolution motion for the controller to track. Besides, the most critical challenge is that jumping motion in the ball bumping task is highly time-sensitive. The quadruped needs to reach the desired takeoff state accurately at a specific time; otherwise, the ball will deviate a lot from the goal. According to this property, there should be a strict time-state constraint imposed on the take-off state.
To fulfill the requirements of the jumping motion, we modify the famous MPC controller [1] for our problem. min xc,uc four ground reaction forces produced by legs is the system input u c T denotes the Euler angles representing the quadruped's orientation. A ∈ R 13×13 and B ∈ R 13×12 are the parameter matrices of discrete 3D-SRBM. Compared to the standard formula of MPC, we transfer the strict time-state constraints of the jumping motion into the cost, which is set as a terminal cost for the takeoff state.
1) Effects of the Terminal Cost: The performance of the convex MPC has been validated in the MIT Cheetah 3 [1] for high-speed running. However, for the ball bumping motion, which is time-sensitive, we need to make sure the quadruped body can reach the desired position and velocity at the desired time instance.
To solve this issue, we add a terminal cost Qp to help the robot to reach the desired hitting state in time. Both Q p and Q are diagonal matrices, every diagonal entries of Q p need to greater than Q's relatively. This terminal cost can help the controller finds a sequence of control inputs that will guide the system to reach the terminal state at the exact time t t rather than ahead of t t .
2) Dynamic Horizon Tuning: During the trajectory tracking process, we will tune the horizon and timestep dt of the discrete SRBM in order to make sure x * q (t t ) computed by the previous motion planner is always the terminal reference state for the jumping controller. We also use horizon cutting and dynamic dt to make sure x * q (t t ) is always the terminal reference state of the controller. This tuning method is described in Algorithm 1.

Algorithm 1: Dynamic horizon tuning
Data: current time t now , takeoff time t t , horizon h, Finally, the revised MPC problem can be formulated as a quadratic programming problem and quickly solved. Then with the foot jacobian of each leg J i ∈ R 3×3 , joint torques of each leg i can be given by

B. Flying Controller
As described in II-A.2, the motion trajectory between the takeoff and collision is considered a parabola. Then, to reduce the effect of the leg motion further, we freeze all legs during this process, which means we use the PD control to keep all joint angles constant.
After the collision t h and a small duration of delay, in order to adjust the leg configuration for landing, we switch to the falling controller; the controller uses Cartesian impedance control: to move the foot to desired locations given by where p is the robot COM position in the world frame and p 0 i is the foot position relative to the COM, its x, y axis values are set manually for a stable landing configuration, and the z axis value is the same as the relative z position at the moment of takeoff.

C. Landing Controller
When any leg contact with the ground is detected by the measurement of the foot contact sensor, the controller uses joint space PD control to damp the robot and finally uses MPC described in [1] to squat and lie on the ground.

A. Robot Platform
The whole motion planning and control framework was validated on the real robot platform Aliengo [26]. Aliengo from Unitree Robotics is a quadruped robot with 12 actuators, whose legs are designed as open chains, and it has a mass of about 22 kg with lightweight legs, so this mechanical design is suitable for the single rigid body model (SRBM) in our methodology. The physical parameters of Aliengo can be found at [26].

B. Experiment Setup
The motion planning and control framework was developed with C++ based on ROS and ros_control [27], which provide hardware interface and controller manager and make simulation (using Gazebo [28]) and debugging efficiency. pinocchio [29] and Eigen are used as basic kinodynamics interfaces in the implementation of the methodology. As for the nonlinear optimization programming for the planner, CasADi [30] was used for constructing the trajectory optimization problem, and IPOPT solver was used for solving it. Quadruped's body mass m was set to 35 kg for increased gains of the system due to the large latency (about 8 ms loop-back) caused by unitree_legged_sdk.
The hardware setup is shown in Fig. 1; the software described above was running on a laptop with an i7-8550U processor and runs Ubuntu Linux with a kernel patched with CONFIG_PREEMPT_RT. Aliengo was controlled by UDP through an Ethernet cable with a 1000 Hz control loop. We made a tennis ball releaser to guarantee the ball's released time, position, and velocity. The ball releaser was controlled by the laptop with a USB to CAN cable.

1) NLP Test:
In order to test the performance of the NLP solver for the planner, we did 10000 test cases with 10 evenly distributed samples per interval over the initial state and desired state x b,1 (0) ∈ [0, 0.5], x b,2 (0) ∈ [1.5, 2.0], x des b,1 ∈ [0.5, 1.5], x des b,2 ∈ [0, 0.5]. All NLPs were solved with a cold start, and the initial guess is set   solving time is about 50ms, most of the solve time is around 80ms, and the maximum solve time is under 120ms, allowing for real-time solving after the ball is released.   I) with the same initial ball release height z = 1.56m and different horizontal positions x. The whole experiment results are included in the supplemental video. The actual ball landing point in all tests is within a circle with a radius of 10 cm centered on the desired landing point. Fig. 6 shows the snapshot of the case with initial ball position x = 0 m,z = 1.56 m and desired landing point x = 2.5 m, z = 0 m, Fig. 7 shows the corresponding pose and twist trajectories given by state estimation module. The ball was released at 0.262 s, and the time instances of a jump start and takeoff were given by solving the nonlinear optimization problem described in section III. The NLP solver spent about 60ms to get the optimized time instances. The quadruped started jumping at 0.342 s and took off at 0.691 s. Finally, the robot reached the desired takeoff state at 0.691 s with the jumping controller described in section IV-A. Fig. 8 shows the foot force computed by MPC in the jumping process. Although the trajectory has not always been tracked very well, the error of takeoff state is acceptable relatively. The state estimation data can not be trusted after the jumping because the foot is off the ground.

VI. CONCLUSIONS AND FUTURE WORK
This paper presented a motion planning and control framework for the ball bumping task that leads the quadruped bump of the ball into the desired landing region. A nonlinear optimization problem is formulated for a dynamically feasible jumping trajectory, which can be solved online. The multiphase controller is designed for this time-sensitive trajectory, which can reach the desired takeoff state at desired time instance accurately. And the performance has been validated on the Aliengo.
Future work can be devoted to the warm-start interface of the nonlinear optimization problem to get a more reasonable initial guess. A vision detection and tracking system can be developed and run onboard, allowing the dropping of the ball by human hand without the releaser. In addition, the planning for landing motion can be developed for a more steady landing performance.