Fast Manipulability Maximization Using Continuous-Time Trajectory Optimization

A significant challenge in manipulation motion planning is to ensure agility in the face of unpredictable changes during task execution. This requires the identification and possible modification of suitable joint-space trajectories, since the joint velocities required to achieve a specific endeffector motion vary with manipulator configuration. For a given manipulator configuration, the joint space-to-task space velocity mapping is characterized by a quantity known as the manipulability index. In contrast to previous control-based approaches, we examine the maximization of manipulability during planning as a way of achieving adaptable and safe joint space-to-task space motion mappings in various scenarios. By representing the manipulator trajectory as a continuous-time Gaussian process (GP), we are able to leverage recent advances in trajectory optimization to maximize the manipulability index during trajectory generation. Moreover, the sparsity of our chosen representation reduces the typically large computational cost associated with maximizing manipulability when additional constraints exist. Results from simulation studies and experiments with a real manipulator demonstrate increases in manipulability, while maintaining smooth trajectories with more dexterous (and therefore more agile) arm configurations.Comment: In Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS'19), Macau, China, Nov. 4-8, 201

Role Playing Learning for Socially Concomitant Mobile Robot Navigation

In this paper, we present the Role Playing Learning (RPL) scheme for a mobile robot to navigate socially with its human companion in populated environments. Neural networks (NN) are constructed to parameterize a stochastic policy that directly maps sensory data collected by the robot to its velocity outputs, while respecting a set of social norms. An efficient simulative learning environment is built with maps and pedestrians trajectories collected from a number of real-world crowd data sets. In each learning iteration, a robot equipped with the NN policy is created virtually in the learning environment to play itself as a companied pedestrian and navigate towards a goal in a socially concomitant manner. Thus, we call this process Role Playing Learning, which is formulated under a reinforcement learning (RL) framework. The NN policy is optimized end-to-end using Trust Region Policy Optimization (TRPO), with consideration of the imperfectness of robot's sensor measurements. Simulative and experimental results are provided to demonstrate the efficacy and superiority of our method

Accelerating Motion Planning via Optimal Transport

Motion planning is still an open problem for many disciplines, e.g., robotics, autonomous driving, due to their need for high computational resources that hinder real-time, efficient decision-making. A class of methods striving to provide smooth solutions is gradient-based trajectory optimization. However, those methods usually suffer from bad local minima, while for many settings, they may be inapplicable due to the absence of easy-to-access gradients of the optimization objectives. In response to these issues, we introduce Motion Planning via Optimal Transport (MPOT) -- a \textit{gradient-free} method that optimizes a batch of smooth trajectories over highly nonlinear costs, even for high-dimensional tasks, while imposing smoothness through a Gaussian Process dynamics prior via the planning-as-inference perspective. To facilitate batch trajectory optimization, we introduce an original zero-order and highly-parallelizable update rule: the Sinkhorn Step, which uses the regular polytope family for its search directions. Each regular polytope, centered on trajectory waypoints, serves as a local cost-probing neighborhood, acting as a \textit{trust region} where the Sinkhorn Step "transports" local waypoints toward low-cost regions. We theoretically show that Sinkhorn Step guides the optimizing parameters toward local minima regions of non-convex objective functions. We then show the efficiency of MPOT in a range of problems from low-dimensional point-mass navigation to high-dimensional whole-body robot motion planning, evincing its superiority compared to popular motion planners, paving the way for new applications of optimal transport in motion planning.Comment: Published as a conference paper at NeurIPS 2023. Project website: https://sites.google.com/view/sinkhorn-step

Path planning algorithms for autonomous navigation of a non-holonomic robot in unstructured environments

openPath planning is a crucial aspect of autonomous robot navigation, enabling robots to efficiently and safely navigate through complex environments. This thesis focuses on autonomous navigation for robots in dynamic and uncertain environments. In particular, the project aims to analyze the localization and path planning problems. A fundamental review of the existing literature on path planning algorithms has been carried on. Various factors affecting path planning, such as sensor data fusion, map representation, and motion constraints, are also analyzed. Thanks to the collaboration with E80 Group S.p.A., the project has been developed using ROS (Robot Operating System) on a Clearpath Dingo-O, an indoor mobile robot. To address the challenges posed by unstructured and dynamic environments, ROS follows a combined approach of using a global planner and a local planner. The global planner generates a high-level path, considering the overall environment, while the local planner handles real-time adjustments to avoid moving obstacles and optimize the trajectory. This thesis describes the role of the global planner in a ROS-framework. Performance benchmarking of traditional algorithms like Dijkstra and A*, as well as other techniques, is fundamental in order to understand the limits of these methods. In the end, the Hybrid A* algorithm is introduced as a promising approach for addressing the issues of unstructured environments for autonomous navigation of a non-holonomic robot. The core concepts and implementation details of the algorithm are discussed, emphasizing its ability to efficiently explore continuous state spaces and generate drivable paths.The effectiveness of the proposed path planning algorithms is evaluated through extensive simulations and real-world experiments using the mobile platform. Performance metrics such as path length, execution time, and collision avoidance are analyzed to assess the efficiency and reliability of the algorithms.Path planning is a crucial aspect of autonomous robot navigation, enabling robots to efficiently and safely navigate through complex environments. This thesis focuses on autonomous navigation for robots in dynamic and uncertain environments. In particular, the project aims to analyze the localization and path planning problems. A fundamental review of the existing literature on path planning algorithms has been carried on. Various factors affecting path planning, such as sensor data fusion, map representation, and motion constraints, are also analyzed. Thanks to the collaboration with E80 Group S.p.A., the project has been developed using ROS (Robot Operating System) on a Clearpath Dingo-O, an indoor mobile robot. To address the challenges posed by unstructured and dynamic environments, ROS follows a combined approach of using a global planner and a local planner. The global planner generates a high-level path, considering the overall environment, while the local planner handles real-time adjustments to avoid moving obstacles and optimize the trajectory. This thesis describes the role of the global planner in a ROS-framework. Performance benchmarking of traditional algorithms like Dijkstra and A*, as well as other techniques, is fundamental in order to understand the limits of these methods. In the end, the Hybrid A* algorithm is introduced as a promising approach for addressing the issues of unstructured environments for autonomous navigation of a non-holonomic robot. The core concepts and implementation details of the algorithm are discussed, emphasizing its ability to efficiently explore continuous state spaces and generate drivable paths.The effectiveness of the proposed path planning algorithms is evaluated through extensive simulations and real-world experiments using the mobile platform. Performance metrics such as path length, execution time, and collision avoidance are analyzed to assess the efficiency and reliability of the algorithms

Combining Subgoal Graphs with Reinforcement Learning to Build a Rational Pathfinder

In this paper, we present a hierarchical path planning framework called SG-RL (subgoal graphs-reinforcement learning), to plan rational paths for agents maneuvering in continuous and uncertain environments. By "rational", we mean (1) efficient path planning to eliminate first-move lags; (2) collision-free and smooth for agents with kinematic constraints satisfied. SG-RL works in a two-level manner. At the first level, SG-RL uses a geometric path-planning method, i.e., Simple Subgoal Graphs (SSG), to efficiently find optimal abstract paths, also called subgoal sequences. At the second level, SG-RL uses an RL method, i.e., Least-Squares Policy Iteration (LSPI), to learn near-optimal motion-planning policies which can generate kinematically feasible and collision-free trajectories between adjacent subgoals. The first advantage of the proposed method is that SSG can solve the limitations of sparse reward and local minima trap for RL agents; thus, LSPI can be used to generate paths in complex environments. The second advantage is that, when the environment changes slightly (i.e., unexpected obstacles appearing), SG-RL does not need to reconstruct subgoal graphs and replan subgoal sequences using SSG, since LSPI can deal with uncertainties by exploiting its generalization ability to handle changes in environments. Simulation experiments in representative scenarios demonstrate that, compared with existing methods, SG-RL can work well on large-scale maps with relatively low action-switching frequencies and shorter path lengths, and SG-RL can deal with small changes in environments. We further demonstrate that the design of reward functions and the types of training environments are important factors for learning feasible policies.Comment: 20 page