We propose a hierarchical task and motion planning (TAMP) for autonomous construction that manipulates deformable objects, such as terrain excavation. The TAMP is required to generate an efficient task plan to meet high-level construction goals at sites with environmental diversity while ensuring motion feasibility. The difficulty, however, is to manipulate deformable objects containing nonlinear dynamics with a target given by a continuous value as the task specification. Optimization-based TAMP with signal temporal logic specifications in robotics is promising because of its continuous task specification and formulation as a nonlinear programming problem. The key to its application to extensive, long-term planning at real construction sites is a computationally efficient and stable formulation. We introduce a new expression for deformable objects with a simple differentiable function and a system model that can represent mode transitions based on machine action on the objects. This allows TAMP to be formulated as simultaneously selecting an action for objects and planning the motion to execute it. Furthermore, a hierarchical method that gives appropriate initial values is combined to improve optimality for large-scale nonlinear problems. From the verification by numerical experiments, the proposed method can generate a plan that minimizes the time to meet the task goal, even when the area is expanded.

Reinforcement learning with domain randomization (DR) has been proposed as a promising approach for learning robust policies to environmental changes. However, for DR to work well in real-world environments, it is necessary to design appropriate DR distributions for model parameters. This paper proposes Normalizing Flow-based Parameter Distribution Estimation (NFPDE), a new estimation method for DR distributions. NFPDE models the target distribution by a flow-based generative model using normalizing flow and estimates the target distribution based on an offline dataset collected a priori in the target environment. Through numerical experiments on the OpenAI gym environment, we show that NFPDE can estimate the target distribution more accurately and efficiently than the previous estimation methods. We also show that the estimated DR distributions can improve the robustness of trained policies.

We present a formulation for a linear temporal logic (LTL)-based task planning using the Koopman operator. The dynamics of nonlinear systems can be represented as linear systems by lifting them to a space of augmented states using the Koopman operator. On the other hand, the lifted linear system cannot capture the nonlinear effects of inputs, which appear in many robotic systems. Therefore, instead of a lifted linear system, we can consider representing control-affine bilinear systems. However, since the lifted bilinear systems are nonlinear, we need to solve nonlinear programming problems for trajectory optimization. This paper presents a methodology for the trajectory optimization problem of the lifted bilinear system. Using the mixed-integer convex approximation, we can solve the trajectory optimization problem of the lifted bilinear systems as a mixed-integer linear programming problem. This formulation allows us to solve LTL-based task planning problems for nonlinear systems. The effectiveness of the proposed method was confirmed by numerical simulations.

Hierarchical algorithms have often been used to plan and execute complicated robotic sequential manipulation tasks, where an abstract planner searches for a skill sequence in an abstract space, and each skill generates actual motions on the basis of the planned skill sequences. To generate executable plans, the abstract planner should know the pre-/postconditions of each skill and appropriately choose skills so that the generated plan satisfies their pre-/postconditions. For such hierarchical planning, this paper presents a novel method for robot skill learning that learns not only a control policy but also the learned skill's pre-/postconditions to complete a given task. Our method combines an optimal control method and an active learning approach called level set estimation (LSE) to effectively collect training data for learning control policies and pre-/postconditions. Although there exists a LSE-based policy learning algorithm that identifies preconditions, its performance is limited to cases where the dimension of the search space for pre-/postconditions is low. The main contribution of this paper is the proposal of a new learning method that can handle tasks having a high-dimensional search space for pre-/postconditions. We demonstrate our proposed method in two robotic tasks. The results show that our method can more effectively learn a control policy and its pre-/postconditions compared with the existing LSE-based method.

We propose a fast optimization algorithm to find feasible solutions for a special class of switching structured problems. A type of continuous optimization problem that has switching structured constraints is called mathematical programs with switching constraints (MPSC). Relaxation methods are well known gradient descent-based approaches for solving MPSC. However, with the use of these conventional algorithms, we reveal that the sequence of solutions easily converges to an infeasible stationary point due to a special class of switching constraint whose feasible set is geometrically separated into mutually exclusive sets in variable space. We define this kind of switching constraint as the disjunctive allowable set constraint (DAS-constraint). To force the sequence of solutions to escape from such infeasible stationary points, we construct a new algorithm that introduces random sampling if convergence to an infeasible point is detected. Furthermore, to reduce the computation cost due to random sampling, we randomize only specific variables that are relevant to DAS-constraints. Numerical experiments show that feasible solutions can be found by using our proposed algorithm, even for large-scale optimization problems where no solutions are found within a practical time limit when using a conventional algorithm.

We propose a new optimization-based task and motion planning (TAMP) with signal temporal logic (STL) specifications for robotic sequential manipulation such as pick-and-place tasks. Given a high-level task specification, the TAMP problem is to plan a trajectory that satisfies the specification. This is, however, a challenging problem due to the difficulty of combining continuous motion planning and discrete task specifications. The optimization-based TAMP with temporal logic specifications is a promising method, but existing works use mixed integer problems (MIP) and do not scale well. To address this issue, in our approach, a new hybrid system model without discrete variables is introduced and combined with smooth approximation methods for STL. This allows the TAMP to be formulated as a nonlinear programming problem whose computational cost is significantly less than that of MIP. Furthermore, it is also possible to deal with nonlinear dynamics and geometric constraints represented by nonlinear functions. The effectiveness of the proposed method is demonstrated with both numerical experiments and a real robot.

We present a formulation for linear temporal logic (LTL)-based task planning of nonlinear dynamical systems. We consider pick-and-place task planning as a typical example of the planning task that can be modeled as a hybrid system that includes the states of robots and objects. LTL-based planning for hybrid systems is solved as a mixed-integer problem (MIP), especially a mixed-integer linear programming problem (MILP). Due to the formulation by the MILP, we could only deal with linear systems and linear constraints. In our proposed method, we apply a convex approximation to systems that have bilinear terms and quadratic terms in their dynamics. And we incorporate nonlinear systems into existing LTL-based planning as an MILP. We demonstrate the effectiveness through numerical simulations of a simple robot arm system and drone system.

Biped robots are expected to have various activities in human society, and many biped robots have been developed. As the example, limit cycle walkers, can realize efficient walking using their own dynamics. However, they have a problem of weak robustness on uneven terrains. On the other hand, a technique for improving robustness by using output zeroing control with relative degree 3 has been proposed. In this paper, we derive the reference trajectory for the output function and holonomic constraints used when applying output zeroing control with relative degree 3 by direct collocation method which is a method of a trajectory optimization. We show that they walk by using the derived reference output function and holonomic constraints and improve the robustness on uneven terrain.

For underactuated bipedal robots, it is important to compensate disturbances which affects the zero dynamics to realize a stable locomotion when we use output zeroing controllers. In order to deal with such disturbances, this paper presents a framework of a periodic trajectory planning and a robust output zeroing controller using a disturbance model learned by Gaussian process regression (GPR). In particular, we propose a control method considering the modeling uncertainty of the disturbance model by using a variance information of GPR model. We show the effectiveness of the proposed method through a numerical simulation of walking control of an underactuated robot model affected by an external force.