Efficient Trajectory Generation for Robotic Systems Constrained by Contact Forces
Jaemin Lee Efstathios Bakolas Luis Sentis




                                              
                                              
                                              

 


                                                

[footnoteinfo]Some part of this paper will be presented at American Control Conference 2019 <cit.>. Corresponding author L. Sentis.

ME]Jaemin Leejmlee87@utexas.edu,    
ASE]Efstathios Bakolasbakolas@austin.utexas.edu,               
ASE]Luis Sentislsentis@austin.utexas.edu  

[ME]Department of Mechanical Engineering, The University of Texas at Austin, TX, 78712-1221, USA  
[ASE]Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712-1221, USA             


          
                           
Robots; Reachability; Contact Force.               
                             
                                          
                                          


                          
In this work, we propose a trajectory generation method for robotic systems with contact force constraint based on optimal control and reachability analysis. Normally, the dynamics and constraints of the contact-constrained robot are nonlinear and coupled to each other. Instead of linearizing the model and constraints, we directly solve the optimal control problem to obtain the feasible state trajectory and the control input of the system. A tractable optimal control problem is formulated which is addressed by dual approaches, which are sampling-based dynamic programming and rigorous reachability analysis. The sampling-based method and Partially Observable Markov Decision Process (POMDP) are used to break down the end-to-end trajectory generation problem via sample-wise optimization in terms of given conditions. The result generates sequential pairs of subregions to be passed to reach the final goal. The reachability analysis ensures that we will find at least one trajectory starting from a given initial state and going through a sequence of subregions. The distinctive contributions of our method are to enable handling the intricate contact constraint coupled with system's dynamics due to the reduction of computational complexity of the algorithm. We validate our method using extensive numerical simulations with a legged robot.






§ INTRODUCTION


This paper considers the optimal control of robotic systems with contact force constraints. Often, it is required that legged or humanoid robots maintain stable foot or body contacts while executing given tasks. In such cases, contact forces constrain and determine the robot's state reachability together with other state and input constraints. Therefore, we seek to devise control algorithms that can generate trajectories for contact-constrained robots via formal state reachability analysis. Often, control studies for robotics assume that task trajectories are predefined <cit.>, then attempt to find an instantaneously optimal solution to accomplish them. However the desired trajectories are frequently infeasible and it is not straight-forward to check the feasibility of trajectories under contact constraints a priori. Many motion planning and trajectory generation approaches for humanoid robots use very simple models of a robot such as considering center of mass dynamics under contact constraints <cit.>. However, those methods result on lower performance of the robots since they cannot capture the robot's kinematics or input constraints among other limitations. 


Optimal control is an alternative approach to solve the trajectory generation problem for contact-constrained robots. Recently, trajectory generation for the legged robots was formulated as a bi-level optimization problem and solved by an iterative Linear Quadratic Regulator (iLQR) <cit.>. However, the iLQR has still challenging issues to address generic nonlinear constraints without constraint softening. This could result on motion planers that violate important physical constraints of robots. To strictly consider the constraints and nonlinearity of the robot models, we should directly solve the optimal control problem via Nonlinear Programming (NLP) in the process of obtaining feasible trajectories. Although many NLP solvers, i.e. SNOPT <cit.> and IPOPT <cit.>, are available, NLP has significantly challenging issues. One is high computational cost for obtaining the solution. Also, feasible initial conditions are necessary for NLP. In our work, we propose two complementary processes to resolve these problems: reachability analysis and sampling-based dynamic programming. 


The reachability problem consists of checking whether the state of the system can reach a specific state over a finite time horizon, starting from a given initial state. The field of robotics has often focused on configuration space reachability guided by the given tasks such as selecting the stance location of humanoid robots <cit.>. Although these methods are useful for kinematic feasibility, they are limited to address the requirements of dynamical systems considering various constraints such as joint velocity/torque limits and contact force constraints. To address this gap, we will employ optimal control on the nonlinear dynamical system with constraints. 

In optimal control, the reachability analysis has been often used for nonlinear systems <cit.>, hybrid dynamical systems <cit.>, and stochastic systems <cit.>. We can categorize established reachability analysis methods for nonlinear systems into three groups: 1) solving Hamilton-Jacobi-Bellman PDE, 2) using linearization and mathematical approximation 3) using propagation and mappings of a set of reachable states. First, for low dimensional dynamical systems, the reachability analysis is often achieved via Hamilton-Jacobi-Bellman PDE <cit.>. For some systems, it is impossible to perform the reachability analysis by solving Hamilton-Jacobin-Bellman PDE. Instead, many approaches have been proposed to compute reachable sets exploiting mathematical techniques, optimization, inherent characteristics of systems, etc.     


Other than the methods using Hamilton-Jacobi-Bellman PDE, many methodologies have been proposed to obtain reachable sets of systems. The logarithmic norm of a type of system's Jacobian is utilized to obtain over-approximated reachable sets for nonlinear continuous-time systems <cit.> and that norm is utilized for simulation-based reachability analysis <cit.>. Another approach tries to do more accurate reachability analysis for uncertain nonlinear systems by using more conservative approximations <cit.>. Also, for continuous-time piecewise affine systems, linear matrix inequalities (LMI) are employed to characterize the bounds of reachable regions <cit.>. Another class of  reachability analysis uses convex sets for approximation such as ellipsoid <cit.>, polytopes, zonotopes <cit.>, and support functions <cit.>. Although those approximation-based approaches are capable of extending to nonlinear systems, they only consider convex sets and often ignore other constraints. These are challenges for extending the previous approaches to more sophisticated and complicated systems. Additionally, computational complexity exponentially increases with respect to the dimension of the state space and the time length for those methods.


Our problem considers a constrained nonlinear system with a constrained variable, e.g. a contact force, coupled with system dynamics. Since the contact force is time varying and our problem is also high dimensional, it is very difficult to do reachability analysis of our system via Hamilton-Jacobi-Bellman PDE. Linearization of dynamics and approximating reachable sets with convex sets is not applicable to our problem because reachable sets of constrained nonlinear systems may not be convex. Moreover, we want to be strictly compared to methods softening or linearizing constraints like iLQR. Thus, we devise a new method consisting of propagating system states and approximating the reachable set. NLP using optimal control utilizes the approximated reachable set. In order to address the increasing computational complexity, we propose various techniques which are described thereafter. And, we do so, in the context of robotic systems with contact force constraints, in a computational efficient way.  

Concretely, we incorporate a sampling-based approach, quadratic programming (QP), NLP, and approximation techniques such as propagation of boundary samples to solve our problem. More specifically, for dividing the end-to-end trajectory generation problem, we obtain the constrained-state set using a sampling-based approach and QP, then, reformulate the problem as a Partially Observable Markov Decision Process (POMDP) in the system's output space. An optimal Markov policy resulting from the dynamic programming (DP) provides a sequence of output subregions. The sequence of output subregions guides the path of output with avoiding unsafe output regions such as locations of obstacles in the output space. In the next step, we implement a rigorous reachability analysis between given pairs of subregions by propagating the states from the given initial state. In addition, we propose a method to approximate the reachable set using propagation of boundary states. The algorithmic efficiency of our method is one of the contributions of our work. 

This paper is organized as follows. Section <ref> defines our problem and the target class of system. A sampling-based algorithm for obtaining the set of constrained states in Section <ref> and a POMDP for obtaining an optimal Markov policy are described in Section <ref>. In Section <ref>, we propose an approach to obtaining the reachable set and analyzing the method in detail. Based on the result of rigorous reachability analysis in Section <ref>, an optimal controller is designed by implementing the NLP of Section <ref>. The proposed approach is validated by simulation of a robotic legged system with contact force constraint in Section <ref>.   



§ PROBLEM FORMULATION





 §.§ Notation

We denote the set of real n-dimensional vectors and the set of real n× m matrices by ℝ^n and ℝ^n × m, respectively. The sets of non-negative and non-positive real numbers are represented as ℝ_≤ 0 and ℝ_≥ 0, respectively. The set of natural numbers and the set of integer numbers are denoted by ℕ and ℤ, respectively. The set of positive definite n× n matrices and the set of positive semi-definite n× n matrices are denoted by 𝕊_>0^n and 𝕊_≥ 0^n. When considering z_1, z_2 ∈ℕ with z_2>z_1, the discrete interval between z_1 and z_2 is defined as [z_1, z_2]_ℕ{z_1, z_1+1, …, z_2-1, z_2}. In case of real numbers z_1, z_2 ∈ℝ_≥ 0, [z_1, z_2]_d^Δ{ z_1, z_1+Δ ,…, z_2-Δ, z_2} denotes a discrete interval with Δ being the increment. When n real numbers a_1,…, a_n are consider, Vec[a_i]_i=1^n∈ℝ^n represents a vector whose i-th element is a_i. Given n× m real numbers a_11,…,a_mn, a matrix whose (i,j) element is a_ij is denoted by Mat[a_ij]_i,j=1^n,m∈ℝ^n× m. Given a square matrix 𝐀∈ℝ^n× n, tr(𝐀) denotes its trace. σ(𝐀) and σ(𝐀) represent the largest and smallest singular values of 𝐀, respectively. Given matrices 𝐀_i∈ℝ^n_i× m i ∈ [1,z]_ℕ, Vertcat(𝐀_1, …, 𝐀_z) ∈ℝ^(n_q +…+n_z)× m indicates a block matrix constructed by vertically concatenating the matrices 𝐀_i i ∈ [1,z]_ℕ. Given a set of real vectors 𝒜⊆ℝ^n, card(𝒜) denotes its cardinality. When considering particular cases such that 𝒜⊂ℝ^n with n ∈ [1,3]_ℕ, ghull(𝒜) and gbd(𝒜) represent the general hull and the set of vectors closest the boundary of 𝒜. 𝔼[.] represents the probabilistic expectation operator. 





 §.§ Nonlinear System Model

We characterize the equation of motion for general robotic systems with contact forces and assuming rigid body linkages as follows:

    𝐌(q)q̈ + g(q̇, q) = 𝐒^⊤u + 𝐉_c^⊤(q) F_c

where q∈ℝ^n_q, 𝐌(q)∈𝕊_>0^n_q, g(q̇,q)∈ℝ^n_q, 𝐒∈ℝ^n_q × n_u, u ∈ℝ^n_u, 𝐉_c(q)∈ℝ^n_c × n_q, and F_c∈ℝ^n_c denote the joint variable, sum of Coriolis/centrifugal and gravitational forces, selection matrix for the actuation, input actuating joint torques, contact Jacobian matrix, and contact force, respectively. We can bring the differential equation (<ref>) into a state space form by defining the state x  [ x_1^⊤ x_2^⊤ ]^⊤∈ℝ^n_x where x_1 = q and x_2 = q̇:


    ẋ(t)    = f_ℭ (x(t), u(t), F_c(t)) 
       = f_x(x(t)) + f_u(x(t))u(t) + f_c(x(t)) F_c(t) 
    
        y(t)    = f_y(x(t)) 
    
        f_x(x)    [[               x_2(t); -𝐌^-1(x_1)g(x_1,x_2) ]]
    
        f_u(x)    [[  0_n_q × n_u; 𝐌^-1(x_1)𝐒^⊤ ]], f_c(x) [ [         0_n_q × n_c; 𝐌^-1(x_1)𝐉_c^⊤(x_1) ]]


where f_x:ℝ^n_x↦ℝ^n_x, f_u:ℝ^n_x↦ℝ^n_u, and f_c:ℝ^n_x↦ℝ^n_c are nonlinear functions of the state x. f_y: ℝ^n_x↦ℝ^n_y is a nonlinear output function that represents the desired tasks, i.e. forward kinematics of points of robot that we wish to control. We define a discrete time state space model of the contact-constrained robotic system (DTSCR) as the discrete counterpart of the state space model:

    x(t_k+1)    = f_𝔇(x(t_k),u(t_k),F_c(t_k) ) 
       = x(t_k) + ∑_i=1^∞B_i(x(t_k),u(t_k), F_c(t_k))/i! (Δ t)^i

where Δ t denotes the sampling period for the discretization of the model. f_𝔇:ℝ^n_x+n_u+n_c↦ℝ^n_x and B_i(x,u,F_c) = ∂ B_i-1(x,u,F_c)/∂ x B_1(x,u,F_c) and B_1(x,u,F_c) = f_x(x)+f_u(x)u + f_c(x)F_c.



 §.§ Constraints of the System

We refer to h_e and h_i as the equality constraint function and the inequality constraint function, respectively, where we assume that h_e(x) = 0 and h_i(x) ≤ 0. Three types of constraint functions are considered in this paper, i.e. functions describing state, the input, and the mixed state-input constraints denoted as h_x(x), h_u(u), and h_xu(x,u), respectively. In practice, the state constraint is introduced to avoid violating joint position or velocity limits when controlling the robot. Input constraints are considered for describing the joint torque limit or the underactuation of floating robots. The mixed state-input constraint contains physically more intricate conditions such as mechanical power. Since the DTSCR is required to maintain stable contact while being controlled, we consider an additional constraint that is known as the contact wrench cone constrained to prevent slip and flip on contact surface. In particular, it is required that 

    h_xc (x,F_c) ≤ 0,    h_xc (x,F_c) 𝐖_c(x) F_c

where 𝐖_c(x) is a matrix describing the unilateral constraint using a polyhedral approximation of the friction cone of a surface <cit.>. The position at the contact point should be constant, which is identical to having null velocity on the robot's contact with respect to the surface contact. The zero velocity constraint corresponds to the state constraint. We will aim at controlling robots represented by the DTSCR model for desired output goals given the aforementioned constraints. 

 



 §.§ Overall Scheme

The proposed approach consists of three methods, which are sampling-based optimization, solving a POMDP, and reachability-based optimal control. The overall procedure is depicted in Fig. <ref>. The first part of the approach aims to obtain feasible states using sampling with respect to the given constraints as shown in Fig. <ref>(a), then to approximately check whether the goal output is feasible via output approximation as shown in Fig. <ref>(b). If the goal is achievable, we formulate and solve a POMDP process to create multiple tractable sub problems as shown in Fig. <ref>(c) and (d). Based on an optimal sequence of nodes, we obtain reachable sets and perform trajectory optimization between neighboring subregions associated with two sequential nodes, then we recursively iterate this process for all sequence of nodes until reaching the final goal output as shown in Fig <ref>(e) and (f). By connecting all trajectories, we obtain the entire trajectory in an efficient way. 



§ SAMPLE-BASED OPTIMIZATION


In this section, we obtain a sequence of subregions in output space considering initial and goal states. Our method breaks down the end-to-end trajectory generation problem into multiple intermediate points sequentially connected. We start by creating random samples from a Gaussian distribution x∼𝒩(μ_x, Σ_x) where μ_x ∈ℝ^n_x and Σ_x ∈𝕊_>0^n_x are the mean and the covariance matrix, respectively, that is, μ_x 𝔼[x] and Σ_x𝔼[(x-μ_x)(x-μ_x)^⊤].



 §.§ Update for Random State Samples

We consider a set of states fulfilling desired constraints. Since the Monte-Carlo method is very inefficient, we apply a least-square QP process to obtain feasible states. Let us consider n_e^c state equality constraints and n_i^c state inequality constraints. We define constraint functions as h_x,e[k_e](x) and h_x,i[k_i](x) where k_e ∈ [1,n_e^c]_ℕ and k_i ∈ [1,n_i^c]_ℕ are indices. Let us define Jacobian matrices and error vectors as follows:


    𝐉_e(x) Vertcat(J_e[k](x):∀ k ∈ [1,n_e^c]_ℕ) 
       𝐉_i(x) Vertcat(J_i[k](x):∀ k ∈ [1,n_i^c]_ℕ) 
       𝐞_e(x)  -Vertcat(h_x,e[k](x): ∀ k ∈ [1,n_e^c]_ℕ)
       𝐞_i(x)   v_\ i^int - Vertcat(h_x,i[k](x): ∀ k ∈ [1,n_i^c]_ℕ)


where J_e[k](x) ∂ h_x,e[k]/∂ x(x) and J_i[k](x)∂ h_x,i[k]/∂ x(x) denote the Jacoabians for equality and inequality constraint functions, respectively. v_\ i^int is an arbitrary interior vector satisfying inequality constraints used as an attractor. The main idea for obtaining the state fulfilling constraints is to iteratively update the sampled state using the state increment Δ x until the constraints are satisfied. The state increment Δ x is obtained by using the QP method as follows:

    min_Δ x,w      w _2^2
    s.t     𝐉_e(x)Δ x ≤𝐞_e(x) + w 
         𝐉_i(x)Δ x ≤𝐞_i(x)

and we update the state x using the optimal variable Δ x. For numerical efficiency, we discard state samples if QP does not converge. Let us define a set 𝒳 consisting of states fulfilling the constraints as follows:

    𝒳{x ∈ℝ^n_x :    h_x,e[k_e] (x) ≤ε,  ∀ k_e∈ [1, n_e^c]_ℕ 
        h_x,i[k_i](x) ≤ 0,  ∀ k_i∈ [1, n_i^c]_ℕ}

where ε is a desired tolerance. 

For the next step, we take all elements of 𝒳 and check whether they fulfill the input, mixed state-input, and contact force constraints. To achieve this, we formulate an optimization problem with a quadratic cost function as follows:

    min _F_c     F_c^⊤𝐐_c F_c + u^⊤𝐐_u u 
    s.t.      x(t_k+1) = f_𝔇(x(t_k),u,F_c)
        h_u[k_u](u) ≤ 0,  ∀ k_u ∈ [1, n_u^c]_ℕ
        h_xu[k_xu](x(t_k), u) ≤ 0,  ∀ k_xu∈ [1, n_xu^c]_ℕ
        h_xc(x(t_k),F_c)  ≤ 0,  x(t_k), x(t_k+1) ∈𝒳

where 𝐐_c∈𝕊^n_c_>0 and 𝐐_u ∈𝕊^n_u_>0 are weighting matrices for the cost. By solving the optimization problem (<ref>) for all x(t_k) ∈𝒳, we obtain a set of states, 𝒳̂, that fulfills all desired constraints.



 §.§ Output Space Approximation

In this subsection, we check the feasibility of reaching the desired goal output. To do so, we approximate the output samples with a Gaussian distribution y^*∼𝒩(μ_y^*, Σ_y^*) <cit.>. The mean and covariance matrix obtained after neglecting higher order terms are 
 

    μ_y^*    f_y(μ_x) + Vec[tr(𝐇_y,i(μ_x)Σ_x)]_i=1^n_y
    Σ_y^*   𝐉_y(μ_x) Σ_x𝐉_y^⊤(μ_x)  
         + 1/2Mat[tr(Σ_x𝐇_y,i(μ_x) Σ_x𝐇_y,j(μ_x))]_i,j=1^n_y,n_y


where 𝐉_y(μ) and 𝐇_y,i(μ) denote the Jacobian matrix of the output function f_y(μ) and the 2nd derivative matrix of the output function f_y,i(μ), for the i-th element. In particular, 

    𝐉_y( μ) ∂ f_y/∂ x (μ),
        𝐇_y,i(μ) [ [      ∂^2 f_y,i (μ)/∂ x_1^2                          … ∂^2 f_y,i (μ)/∂ x_1∂ x_n_x;                          ⋮                          ⋱                          ⋮; ∂^2 f_y,i (μ)/∂ x_n_x∂ x_1                          …    ∂^2 f_y,i (μ)/∂ x_n_x^2 ]]

where 𝐉_u(μ) ∈ℝ^n_y × n_x and 𝐇_y,i(μ) ∈ℝ^n_x × n_x is a symmetric matrix. We construct a probabilistic ellipsoid in the output space to approximate whether an output sample y^* is feasible. We define a set of outputs that lie inside an ellipsoid ℰ_κ with

    ℰ_κ{ y ∈ℝ^n_y: (y - μ_y^*)^⊤Σ_y^*^-1 (y - μ_y^*) ≤κ}

where κ is a coefficient determined by the cumulative probability of the Chi-square distribution. For instance, κ=5.991 for Pr(y^*∈ℰ_κ) = 0.95 and y^*∈ℝ^2. Our method to check if a goal output y_g is interior to ℰ_κ is more efficient than using a Monte Carlo method, because we only need to compute μ_y^* and Σ_y^* using the mean and covariance matrix of the samples using (<ref>). 



§ POMDP FOR A SEQUENCE OF SUBREGIONS


After checking that the desired output goal y_g is located at the interior of the ellipsoid ℰ_κ in (<ref>), the end-to-end trajectory generation problem is divided using intermediate points in the output space. To start the process, we define output subregions:

    𝒴_i{ y ∈ℝ^n_y:   y - y_c,i_∞< ε_y}

where y_c,i∈ℝ^n_y denotes the center of the output subregion 𝒴_i and i∈[1,m]_ℕ where ⋃_i∈ [1,m]_ℕ𝒴_i = 𝒴⊂ℝ^n_y. Also, we obtain a set of outputs 𝒴̂f̂_y(𝒳̂) = { y ∈ℝ^n_y: y = f_y(x) , x ∈𝒳̂} where f̂_y : 𝒳̂⇉𝒴̂. To formulate our problem as a POMDP, we define discrete nodes associated with the previous subregions as follows:

    s_i = node( 𝒴_i) ,    i ∈ [1, m]_ℕ

where 𝐒{ s_1, …, s_m }. Based on these nodes, we transform the problem to a POMDP. We will formulate the probability of observations using the sampled states.


(POMDP) Partially Observable Decision Making Process is defined as a tuple 𝐏 = (𝐒, 𝐀, 𝐎, 𝐓, 𝐙 ):

    
  * 𝐒 is a finite set of nodes, 𝐒{ s_1, ⋯, s_m_s} 
    
  * 𝐀 is a finite set of actions, 𝐀{a_1, ⋯, a_m_a} 
    
  * 𝐎 is a finite set of observations, 𝐎{ o_1, ⋯, o_m_o}
    
  * 𝐓 is the transition dynamics 𝐓(s',s,a) defining the transition from s∈𝐒 to s'∈𝐒 after taking an action a ∈𝐀.
    
  * 𝐙 is the observation 𝐙 (s,a,o) consisting of the probability of observing o∈𝐎 after taking an action a∈𝐀 from node s∈ S.



The problem concerning this section is on finding a sequence of feasible subregions towards an output goal using POMDP tools and analysis. 


(Markov Policy) A Markov policy Π is defined as a sequence: Π{ a^(1), ⋯,  a^(n)}. a^(j)∈𝐀, where a^(j): 𝐒→𝐒 is a measurable map from a node to another one, j∈ [1,n]_ℕ .


We convert the POMDP into a belief MDP. Belief b[s_i] is defined with respect to discrete nodes s_i ∈𝐒. Let suppose b=b[s^(j)], b' = b[s^(j+1)], and a= a^(j) where s^(j) represents the node for the j-th step of the POMDP. The belief transition function, Γ(b,a,b'), is equal to


    Γ(b,a,b') = ∑_o∈𝐎Pr(b'|b,a,o)Pr(o|b,a) 
       Pr(b'|b,a,o) = {[                          1, if belief update returns b';                          0,                   otherwise ]. 
       Pr(o|b,a) = ∑_s'∈𝐒𝐙(s',a,o)∑_s ∈𝐒𝐓(s',s,a)b .


The key challenges of this POMDP are on defining useful observations and on finding their conditional probability. Let us consider that 𝒴 = 𝒴^(j) and 𝒴' = 𝒴^(j+1) associated with the nodes s^(j) and s^(j+1). We propose to define observations as the set of feasible states after taking an action a, i.e.

    Ô   { z ∈ℝ^n_x:  z  = x(t_k),  ∃ (F_c, u)  in  (<ref>)  with 
        f_y(x(t_k)) ∈𝒴∩𝒴̂,   f_y(x(t_k+1) ∈𝒴' ∩𝒴̂}.

where 𝒴 is the subregion before taking the action a. If z_1 ∈Ô, it holds that there exists at least one sample connecting f_y(z_1) to another output in the subregion 𝒴' satisfying the constraints. Considering the above observations, we define the conditional probability as 

    𝐙(s',a,o) Pr(o|s',a)  = card( Ô )/card( 𝒴' ∩𝒴̂).

Let us focus on the reward and transition dynamics. As a heuristic, a higher number of feasible samples falling into a subregion implies a higher probability of reaching it. Therefore we define the reward

    ℜ(s_i,a)  K_rcard( 𝒴_i∩𝒴̂ )/card(𝒴̂) + η_i

where 𝒴_i is the subregion associated with node s_i ∈𝐒. K_r ∈ℝ_>0 and η_i∈ℝ are the gain and reward offset, respectively. We take η_i to be large when y_g∈𝒴_i. Also, to avoid unsafe output regions we set η_i to large negative values.



Consider a node s_i∈𝐒 associated with an output subregion 𝒴_i and a set  𝒴̂ = f_y (𝒳̂). Consider Σ_𝒴_i being the covariance matrix for the set 𝒴_i∩𝒴̂, that is, Σ_𝒴_i=𝔼[(y - μ_y)(y - μ_y)^⊤] and μ_y = 𝔼[y] where y ∈𝒴_i∩𝒴̂. A principle singular vector is defined as

    𝒱(s_i) = col(𝐕_𝒴_i)_k,   σ_k(Σ_𝒴_i) = σ (Σ_𝒴_i) .

where Σ_𝒴_i = 𝐕_𝒴_i^⊤Λ_𝒴_i𝐕_𝒴_i, Λ_𝒴_i = diag(σ_1, …,σ_n_y), and σ_k denotes the singular value of Σ_𝒴_i.


For defining the transition dynamics, let an action a ∈𝐀 map state s ∈𝐒 to s' ∈𝐒. d is a vector in our grid world defined as d  (y_c' - y_c) where y_c and y_c' are the centers of subregions associated with s and s', respectively. We define the transition dynamics as


    𝐓(s',s,a)    {[ 𝒯(s', s, a) / ϖ,         if ϖ≠ 0,;               0,             else ]. 
    𝒯(s',s, a)    max{ 0, 𝒱^⊤(s) 𝐚}
    

where ϖ = ∑_a' ∈𝐀𝒯(s', s, a') denotes a normalization constant.


Let s=s_i∈𝐒 and the corresponding subregion be 𝒴_i. If the output samples are uniformly distributed in 𝒴_i, then 𝐓(s', s, a) =0. 


Since the samples are uniformly distributed, it is possible to select any unit vector in ℝ^n_y as the PSV of s_i, that is, 𝐑_a𝒱(s_i) where 𝐑_a ∈SO(3) is a rotation matrix. If we select the rotation matrix 𝐑_a such that d^⊥ = 𝐑_a 𝒱(s_i), which is orthogonal to 𝒱(s_i), it follows that 𝒯(s',s,a) = 𝒱^⊤(s_i) d = (d^⊥)^⊤ d = 0 for all a ∈𝐀.


We now solve a finite-horizon belief MDP. The optimal policy, denoted by Π_⋆, is obtained by solving the Bellman equation as follows:

    𝔇_⋆(b) = max_a ∈𝐀 [r(b,a) + γ∑_o ∈𝐎Pr(o|b,a) 𝔇_⋆(Γ(b,a,o))]

where r(b,a) = ∑_s ∈𝐒 b(s)ℜ(s,a) denotes the belief reward. The result of the DP provides an optimal Markov policy which we transform to a sequence of nodes as


    Π_⋆   = {a_⋆^(1), ⋯, a_⋆ ^(n_π)}
    𝐒_⋆   = { s_⋆^(1), ⋯, s_⋆^(n_π)}


where a_⋆^(i) and s_⋆^(i) are i-th action and node in a sequence toward reaching the final output goal, respectively. The sequence of subregions in output space is

    𝐘_⋆   = {𝒴_⋆^(1), ⋯, 𝒴_⋆^(n_π)}.

Based on the generated sequence of subregions, we will generate trajectories using reachability analysis connecting subregions in 𝐘_⋆ in the next section. 



§ REACHABILITY ANALYSIS


In this section, we define discrete reachable sets and propose the way to obtain the reachable sets via optimizations. To overcome the computational complexity of propagation algorithm for the reachable sets, a method propagating the boundary samples is proposed and analyzed in the views of computational complexity.   



 §.§ Optimization-based Reachability Analysis

We define reachable sets in continuous time domain as

    ℛ_x^C   (t,x_0) { x(t) : ∃ u([t_0, t]), ∃ F_c([t_0, t])
        h_x[k_x](x(τ)) ≤ 0 ,   ∀ k_x ∈ [1,n_x^c]_ℕ
        h_xu[k_xu](x(τ),u(τ)) ≤ 0,   ∀ k_xu∈ [1, n_xu^c]_ℕ
        h_xc(x(τ),F_c(τ)) ≤ 0,   u(τ) ∈𝒰
       ẋ(τ) = f_𝒞(x(τ), u(τ), F_c(τ)) 
         x(t_0) = x_0 ∈ℛ_x^C(t_0,x_0),  τ∈ [t_0, t]}

ℛ_x^C(t_0, x_0) is equal to { x_0 } which means that the initial state fulfills all constraints.

Let x_0 ∈ℛ_x^C(t_0,x_0), be an initial state and t ∈ [t_0, t_f]_d^Δ t be an arbitrary time interval. We define a reachable set in discrete time domain as:

    ℛ_x^D   (t,x_0) { x(t) : ∃ u([t_0, t]_d^Δ t), ∃ F_c([t_0, t]_d^Δ t) 
        h_x[k_x](x(τ)) ≤ 0 ,  ∀ k_x ∈ [1,n_x^c]_ℕ
        h_xu[k_xu](x(τ),u(τ)) ≤ 0,  ∀ k_xu∈ [1, n_xu^c]_ℕ
        h_xc (x(τ),F_c(τ))  ≤ 0,   u(τ) ∈𝒰
        x(τ + Δ t) = f_𝔇(x(τ),u(τ),F_c(τ)) 
        x(t_0) = x_0 ∈ℛ_x^D(t_0,x_0),  τ∈ [ t_0, t]_d^Δ t}

where Δ t > 0 is the discretization step or sampling period for our discrete model. 

We extend the reachable set defined above for the finite discrete time interval [t_0, t_f]_d^Δ t as ℛ_x^D([t_0,t_f]_d^Δ t,x_0) ⋃_t ∈ [t_0, t_f]_d,Δ tℛ_x^D(t,x_0). For any t_f< ∞, the reachable set satisfies the following bound ℛ_x^D([t_0,t_f]_d^Δ t, x_0) ⊆ℛ_x^D([t_0,+∞),x_0) ⊆ℛ_x^C([t_0,+∞),x_0) ⊊𝒳.

Consider x_0 and ℛ_x^D([t_0, t_k]_d^Δ t, x_0). A random input is drawn from a Gaussian distribution u∼𝒩(μ_u,Σ_u) at each instant of time with the input set 𝒰 defined as the collection of inputs fulfilling input constraint. We define a QP to check for feasible contact forces, i.e.

    min _F_c, Δ x     F_c^⊤𝐐_c F_c + Δ x^⊤𝐐_xΔ x 
    s.t.      x(t_k+1) = f_𝔇(x(t_k),u(t_k),F_c) 
        x(t_k) ∈ℛ_x^D([t_0,t_k]_d^Δ t,x_0)
        h_x[k_x](x(t_k+1)) ≤ 0,  ∀ k_x∈ [1, n_x^c]_ℕ
        h_xu[k_xu](x(t_k),u(t_k) ≤ 0,  ∀ k_xu∈ [1, n_xu^c]_ℕ
        h_xc (x(t_k),F_c) ≤ 0,   u(t_k) ∈𝒰
       Δ x = x(t_k+1) - x(t_k)

The reachable set at the next time instance t_k+1 is obtained by collecting the x(t_k+1) updated by the QP solution Δ x in (<ref>). Then, we can extend the reachable set over [t_0, t_k+1]^Δ t_d such that ℛ_x^D([t_0, t_k+1]_d^Δ t,x_0) ℛ_x^D([t_0,t_k]_d^Δ t,x_0) ∪ℛ_x^D(t_k+1,x_0). Also, we define the set of outputs associated with reachable states such as ℛ_y^D(t,x_0) = f̂_y(ℛ_x^D(t,x_0)) and ℛ_y^D([t_0,t_f]_d^Δ t,x_0) = f̂_y(ℛ_x^D([t_0,t_f]_d^Δ t,x_0)).

 
Suppose that ℛ_x^D (t_0,x_0) is a non-empty set and the desired output, y_d, is given. Let us assume that the set, ℛ_y^D([t_0,t_f]_d^Δ t, x_0), is compact, connected, and

    y_d∈ℛ_y^D([t_0,t_f]_d^Δ t, x_0) .

At least, one state trajectory Ψ{ξ(t_0), ξ(t_1), …, ξ(τ) } exists such that f_y(ξ(τ)) = y_d where τ≤ t_f.


Since ℛ_y^D([t_0,t_f]_d^Δ t, x_0) is compact and f_y is continuous, ℛ_x^D([t_0,t_f]_d^Δ t, x_0) is closed and f_y^-1 is also continuous. Then, ℛ_x^D([t_0,t_f]_d^Δ t, x_0) is connected because ℛ_y^D([t_0,t_f]_d^Δ t, x_0) is connected and f_y^-1 is continuous. Therefore, there exists at least one trajectory connecting x_0 to x_f satisfying f_y(x_f) = y_d in ℛ_x^D([t_0,t_f]_d^Δ t, x_0).   


 
The set, ℛ_y^D([t_0,t_k]_d^Δ t,x_0), is compact.


Let us consider ghull(ℛ_y^D([t_0,t_k]_d^Δ t,x_0)), being compact. By the Heine-Borel theorem, all closed subsets of a compact set are also compact. Since ℛ_y^D([t_0,t_k]_d^Δ t,x_0) ⊂ghull(ℛ_y^D([t_0,t_k]_d^Δ t,x_0)), the reachable set ℛ_y^D([t_0,t_k]_d^Δ t,x_0) is compact.  

 
Suppose that x_0 ∈ℛ_x^D(t_0, x_0) and f_y is continuous. Then, a set, ℛ_y^D([t_0,t_k]_d^Δ t,x_0), is connected. 


Consider three sets: ℱ_1 = ℛ_x^D([t_0,t_k-1]_d^Δ t, x_0), ℱ_2 = ℛ_x^D(t_k, x_0), and ℱ_3 = ℱ_2 ∪ℱ_1', where ℱ_1' is the collection of states x∈ℱ_1 producing the next feasible state via the optimization (27) with respect to x ∈ℱ_1. Then, ℛ_x^D([t_0, t_k]_d^Δ t, x_0)=ℱ_1∪ℱ_2 = ℱ_2∪ℱ_3. Let us consider arbitrary two sets ℋ_1 and ℋ_2 satisfying ℛ_x^D([t_0, t_k]_d^Δ t, x_0) = ℋ_1 ∪ℋ_2 with ℋ_1 ∩ℋ_2 = ∅. Let x_h ∈ F_1' and suppose x_h ∈ℋ_1. Then, ℋ_1 ∩ℱ_1≠∅ and ℋ_1 ∩ℱ_3≠∅. This implies that ℱ_1⊆ℋ_1 and ℱ_3⊆ℋ_1, hence, ℋ_2 = ∅. This proves that ℛ_x^D([t_0,t_k]_d^Δ t, x_0) is connected. Since the mapping f_y is continuous, we also conclude that the set ℛ_y^D([t_0, t_k]_d^Δ t, x_0) is connected. 




 §.§ Propagation of Boundary States

The basic algorithm for reachability analysis suffers from exponential complexity with respect to the number of time steps. Although the previous POMDP contributes to reducing the time horizon to be checked for reachability analysis, full-state propagation would still result in heavy computational burden. In this section, we propose a method for reducing the computational complexity of the algorithm by only propagating selected states. This approach results in more conservative reachable sets. We implement the propagation of boundary states by solving (25) for only state samples x(t_k) ∈ℬ_x^D([t_0, t_k]_d^Δ t,x_0) such that 

    ℬ_x^D([t_0,t_k]_d^Δ t,x_0) {    x ∈ℝ^n_x : x∈ℛ_x^D([t_0,t_k]_d^Δ t,x_0),
        f_y(x) ∈gbd(ℛ_y^D([t_0,t_k]_d^Δ t ,x_0)) }.

We then compute the reachable set by collecting the updated states and extend the time horizon. In this way, the complexity becomes linear with respect to the number of boundary samples, card(ℬ^D([t_0,t_k]_d^Δ t,x_0)). In order to replace full-state propagation with boundary-state propagation, we show that the set of reachable outputs is compact and connected. First, the set ℛ_y^D([t_0,t_k]_d^Δ t,x_0) is compact, because we are able to obtain the hull of the set as shown in Corollary <ref>. Next, we prove the reachable set ℛ_y^D is connected.

    Suppose that x_0 ∈ℛ_x^D(t_0,x_0) ≠∅ and f_y is continuous. Then, ℛ_y^D([t_0,t_k]_d^Δ t,x_0) is connected.


The proof is similar to that of Corollary 2 and therefore is omitted.




 §.§ Computational Complexity Analysis


We analyze the computational complexity to compare the efficiency of the propagation of full states and that of boundary states. There exists many algorithms to obtain the concave hull from the set of data <cit.>. They have 𝒪(n^3) or 𝒪(nlog n) time complexity with n data in 2-D space. General QPs are non-deterministic polynomial-time hard, which means the algorithms are more complex than the polynomial time complexity to be solved. In the case that the QP is convex, it is widely known that the time complexity of the QP is 𝒪(m^3) where m is the number of decision variables.

Based on the aforementioned discussion, we can compare the computational complexity of two cases: propagation of full states and propagation of boundary states. Let us consider N_t steps over the time interval [t_0,t_k]_d^Δ t where Δ t = (t_k- t_0)/N_t, and N_u is the number of input samples. For each propagation method, the computational complexity can be represented as 𝐂_f∼𝒪 (∑_i=1^N_t N_u^i (n_c + n_x)^3 )≈𝒪( N_u^N_t+1 (n_c+n_x)^3), and 𝐂_b∼𝒪 (∑_i=1^N_t N_b(n_c+n_x)^3 + (iN_b)^3) ≈𝒪( N_tN_b(n_c+n_x)^3 + N_t^4N_b^3) 
where 𝐂_f, 𝐂_b, and N_b denote the complexity of full state propagation, that of boundary state propagation, and the number of boundary samples. Normally, a set of boundary samples contains much smaller samples than a set of entire states, that is, N_b ≪ N_u. The effect of the boundary sampling on computational complexity becomes significantly advantageous in terms of the number of time steps. We will show the comparison of the computational complexity using an example in the simulation section.   



§ OPTIMAL CONTROL


In this work, we describe the use of sequential optimal control for nonlinear programs without constraint softening. Instead of considering end-to-end trajectory generation, we focus on finding a trajectory connecting two subregions obtained by the POMDP process described earlier. By iterating this process for connecting subregions (<ref>), the optimal control process is able to attain the desired output with reduced computational cost. 

 



 §.§ Nonlinear Programming

In order to formulate the optimal control problem solved by NLP, a performance measure is defined in the discrete time and state space, that is,


    ℒ(𝐔,N) ∑_k=0^N (u^⊤(t_k)𝐐_uu(t_k) + F_c^⊤(t_k)𝐐_cF_c(t_k) 
                   + e_x^⊤(t_k) 𝐐_x e_x(t_k) ) + e_y^⊤ (t_N) 𝐐_y e_y(t_N) 
        e_x(t_k) ξ(t_k) - x_0 ,       e_y(t_k)  (f_y(ξ(t_k))-y_d)


where 𝐔{ u(t_0), u(t_1), …, u(t_N) }. In addition, 𝐐_x∈𝕊_>0^n_x and 𝐐_y∈𝕊_>0^n_y denote the weighting matrices for the state and output terms, respectively. ξ(t) ∈ℝ^n_x and y_d∈ℝ^n_y denote the trajectory of the state and the desired goal of the output of the NLP, respectively. Based on the set of reachable states, we can formulate the NLP as follows:

    min_Ψ,𝐔     ℒ(𝐔, N) 
    s.t.     ξ(t_k+1) = f_𝔇(ξ(t_k),u(t_k),F_c(t_k)) 
       ξ(t_k) ∈ℛ_x^D([t_0,t_k]_d^Δ t,x_0)
       ξ(t_k+1) ∈ℛ_x^D([t_0,t_k+1]_d^Δ t,x_0) 
         h_x[k_x](ξ(t_k+1)) ≤ 0,  ∀ k_x∈ [1, n_x^c]_ℕ
         h_xu[k_xu](ξ(t_k),u(t_k) ≤ 0,  ∀ k_xu∈ [1, n_xu^c]_ℕ
         h_xc(ξ(t_k), u(t_k)) ≤ 0,   u(t_k) ∈𝒰 .

The proposed approach recursively executes the formulated NLP (<ref>) by changing the initial conditions, the constraints, and the goal output. Let us represent the optimization problem for connecting 𝒴_⋆^(j) to 𝒴_⋆^(j+1) as 

    (Ψ^(j), 𝐔^(j)) = NLP (x_0^(j), y_d^(j), t_0^(j), N^(j))

where j ∈ [1, n_π -1]_ℕ. We replace t_0^(j+1), x_0^(j+1) and y_d^(j+1) with t_N^(j), ξ^(j)(t_N^(j)) and y_c^(j+1), respectively. N^(j+1) is determined by reachability analysis using t_0^(j+1), x_0^(j+1), and y_d^(j+1). In particular, we set x_0^(1) = x_0 and y_d^(n_π -1) = y_g. After performing the iterative NLP, we obtain the entire state and input trajectories to reach the goal output such that


    Ψ^⋆   {Ψ^(1), …, Ψ^(n_π-1)}
    𝐔^⋆   {𝐔^(1),…, 𝐔^(n_π-1)}.


The solutions, Ψ^⋆ and 𝐔^⋆, enable the robotic system to reach the goal output state maintaining all constraints and contacts.  
 


§ NUMERICAL SIMULATIONS


In this section, we validate the proposed approach by simulating a legged robot Draco, which is developed for efficient and dynamic locomotion using liquid-cooled series elastic actuators <cit.>. The dynamic simulation is implemented by DART <cit.>. We utilize two optimizers: Goldfarb for QP and IPOPT implementing a primal-dual interior point method <cit.>. The simulation is executed on a laptop with a Core i7-8650U CPU and 16.0 GB RAM.       



 §.§ A Robotic System and Constraints

A simulation model of Draco consists of three virtual joints, i.e. position and orientation of its floating base (q_1, q_2, q_3) and three actuated joints, i.e. knee and ankle joints (q_4, q_5, q_6) as shown in Fig. <ref> (a). For our simulations, the state and input constraints are defined as follows:

    q_j_1^⊖≤ q_j_1≤ q_j_1^⊕, q̇_j_1^⊖≤q̇_j_1≤q̇_j_1^⊕,   u_j_1^⊖≤ u_j_1≤ u_j_1^⊕
       u_j_2 = 0,    j_1 ∈{4,5,6},    j_2 ∈{1,2,3}

where superscripts (.)^⊖ and (.)^⊕ represent the lower and upper limits, respectively. The specific conditions are q^⊖_j_4,j_5,j_6 = [ - 1.2,    0.5.   -1.5], q^⊕_j_4,j_5,j_6 = [ -0.2,   2.6,   -0.5], u^⊖_j_4,j_5,j_6 = [ -1000,   -1000,   -1000], and u^⊕_j_4,j_5,j_6 = [ 1000,   1000,   1000]. The position, orientation, and velocity of the foot should satisfy the kinematic constraints for the contact. We consider a surface contact with rectangular support polygon on the foot so that the friction cone constraints can be characterized as 

    |f_x| ≤ k_μ f_z,    f_z >0,    | τ _y | ≤ d_x f_z

where d_x denotes the distance between the center of the polygon and the vertex in the local frame of the foot and k_μ represents the friction coefficient. F_c  [f_x, f_z, τ_y ]^⊤ is the contact wrench, which is a resultant contact force at the center of the support polygon. Based on the inequality constraints of (<ref>) and the coordinate transformation from local frame on the foot to the global frame, we can represent the friction cone constraints in the form 𝐖_local𝐑_c(q) F_c = 𝐖_c(q) F_c ≤ 0 where 𝐖_local is a coefficient matrix derived from (<ref>) and 𝐑_c(q) is a rotational matrix from global to foot frames. In 𝐖_local, we set the friction coefficient k_μ = 0.4. Considering all constraints, we will control the robot's motion while maintaining the contact.  




 §.§ Setup for Simulation

To start the reachability analysis, we generate 10^6 state samples and gather the states fulfilling the constraints. The mean and covariance of the Gaussian distribution for sampling are 

    μ_x    = [0.352, 0.384, -0.95, 2.2, -1.25, 0_1× 6]^⊤
    Σ_x   = [[    π𝐈_6× 6    0_6 × 6;    𝐎_6 × 6 2 π𝐈_6 × 6 ]] .

We define a threshold for numerical convergence (<ref>) with value 1.0 × 10^-7 and the maximum number of iterations is 1.0 × 10^6. After implementing the sampling-based approach described in Section <ref>, we obtain 3.47 × 10^5 states among 10^6 state samples. For formulating the POMDP problem in 2D space, we set 40 nodes defined by 𝐒 ={s_i} where i∈ [1,40]_ℕ and 8 actions 𝐀 = { a_j} where j∈ [1,8]_ℕ, and each action consists of moving up, down, right, left, up-right, up-left, down-right, and down-left in the grid world, respectively. We consider two static obstacles for the robot to avoid in the output space. The objective of our numerical simulation is to obtain an optimized trajectory to reach the goal output while avoiding the obstacles and fulfilling all constraints. In addition, we generate 1.0 × 10^5 input samples for propagating the states in the reachability analysis. 



 §.§ Simulation Results

First, the results of our sample-based optimization is shown in Fig. <ref>(b). The set with red dots contains the outputs associated with the states fulfilling the constraints given by the optimization process described in Section <ref>. As shown in Fig. <ref>(b), both the initial [ 0.384 ,   0.352 ] and goal [ 0.51 ,  0.52] outputs are located at the interior of the feasible set. Then, we solve the POMDP problem to find a sequence of nodes, which result in 12 of them, avoiding the obstacles as shown in Fig. <ref>(c). After obtaining the sequence of nodes, we obtain the reachable sets as shown in Fig. <ref>. The reachable set ℛ_y^D([0,0.05]_d^0.001,x_0) contains the desired output associated with the first node. Based on this result, we solve the NLP (<ref>) to find a trajectory to reach the desired output from the initial configuration.         



 

The computational complexity is analyzed by measuring the execution time of the algorithm for computing the reachable sets. We repeat 10 simulations to measure the computation time for both the full-state and boundary-state propagation methods and display the results in Fig. <ref>. The algorithm cannot compute the reachable sets via the full-state propagation method for more than two steps due to the lack of memory capacity as shown in the blue dotted line in Fig <ref>. As we predicted in the complexity analysis of Section <ref>, the boundary-states propagation method significantly reduces the computational time for computing reachable sets.  

Fig. <ref> shows the results of trajectory optimization to reach the goal position with respect to a given initial state. The optimization result includes both joint position and velocity trajectories fulfilling kinodynamic constraints, e.g. joint position, velocity, and torque limits and contact kinematic and force constraints. The optimization results for the actuated joints in the phase space have stabilizable end points which are marked as blue diamonds in Fig. <ref>(a), (b), and (c). As shown in Fig <ref>(d), the generated trajectories pass through the subregions in an optimized sequence obtained by solving the POMPD problem and reaching the final output goal position. 



§ CONCLUSION

This paper proposes a method to generate trajectories for complex robotic systems considering contact constraints. We utilize NLP to solve the optimal control problem. Our approach focuses on efficiently solving the NLP problem so that we can scale the method to many types of complex robotic systems. We devise a new approach to obtain discrete-time reachable sets for trajectory generation and solve the nonlinear optimization problem. Although the computational cost is significantly reduced, it is still challenging to employ this approach to real-time control. Therefore, in the near future, we will investigate ways to combine this approach with feedback controllers and extend the proposed method for hybrid dynamical systems, such as biped humanoid robots or dual arm manipulators. 

                               
The authors would like to thank the members of the Human Centered Robotics Laboratory at The University of Texas at Austin for their great help and support. This work was supported by an NSF Grant# 1724360 and partially supported by an ONR Grant# N000141512507.

























plain        
           
                                 
                                 
                                 


                                 
                                 
                                 
                                 



























