Conclusion

In this chapter, we have developed constraints and methods for path planning for de- formable bodies. Since we use a well-established motion model and realistic physically- driven interactions with the environment, the resulting path is smooth and reasonably “natural” in terms of the generated motion. Furthermore, with physics-based sampling and the accelerated simulation, the planner typically returns a path in a reasonable amount of time.

Figure 4.9: Path Planning of Catheters in Liver Chemoembolization: We highlight the collision-free computed by our algorithm for the catheter shown in Fig. 4.2. We show the overall path from the start to the end configuration in the rightmost image. The left images highlight the zoomed portions of the path, showing bends and deformations.

Chapter 5

Articulated Agents

5.1

Introduction

Modeling and simulation of multi-body dynamical systems has been well-studied due to its wide applications including robotics and automation, molecular modeling, computer animation, medical simulations, and engineering analysis. Examples of highly articu- lated robots include snake or serpentine robots, reconfigurable robots [CB94b,GRC05], and long mechanical chains. In molecular modeling, long series of atoms typically represented by hundreds or thousands of links are commonly used [Sur92]. Catheters [GSLM05b], cables [LK04], and ropes can also be modeled as articulated robots with a very high number of joints.

Highly articulated robots, such as snake or serpentine robots, with many degrees of freedom (DoFs) have recently received considerable attention from the robotics com- munity [CB94c, HHC98, WJC+_{03]. Chirikjian and Burdick first introduced the term} hyper-redundant robots to describe such robots with a very high number of DoFs [CB90, CB94a]. Snake-like robots can serve as suitable alternatives over traditional robotic systems for difficult terrains and challenging scenarios. These include search and rescue missions in complex urban environments, planetary surface exploration, min- imally invasive surgery, or inspection of piping and cabling. Highly articulated robots

also have many applications in homeland security and national defense, as well as en- abling inspection of ships, containers and other structures with narrow, tight workspace. Many computational biology algorithms also model the molecular chains as articulated models with hundreds or thousands of links.

As previously mentioned, randomized motion planning research has made great strides toward providing planning solutions in high dimensional configuration spaces. Probabilistic roadmaps (PRMs), rapidly-exploring random trees (RRTs), and several variants can solve complex problems with dozens of DoFs [KSLO96, KL00]. Since they rely upon randomization to map or explore the configuration space, their performance degrades considerably for robots with much higher dimensionality (e.g. hundreds or thousands) for a couple of reasons[PK06]. As the number of degrees of freedom increases, it becomes more expensive to compute representative samples of the configuration space for these planners. Moreover, dynamics or kinematics constraints on the problem further constrain the search space and make sample generation even more difficult since many planners only consider geometric constraints (such as non-penetration). For instance, consider the motion generated by a either a PRM or RRT. In most variations of these methods, the links are typically generated as straight-line paths in the configuration space using a local planner and only take into account geometric constraints (e.g. col- lision avoidance). As a result, motion is non-smooth when transitioning between links. A local planner will also have to perform additional checks along the link to ensure that kinematics constraints are satisfied. On the other hand, if the motion generated was completely physically-based, the path would be more realistic.

One of the key components of multi-body dynamics simulations is forward dynam- ics computation, which determines the acceleration and resulting motion of each link, given a set of active joint forces and external forces. The optimal algorithms have a linear runtime complexity in the number of degrees of freedom (DOFs). However, these algorithms may still not be sufficiently fast for complex systems that have a very high

number of degrees of freedom. Furthermore, if the environment consists of many obsta- cles or multiple articulated robots, dynamic simulation with robust contact or collision handling can become a major bottleneck for real-time applications.

5.2

Overview

In this section, we define the notation used throughout the chapter and introduce the main ideas needed for physics-based sampling for highly articulated bodies.

5.2.1

Notation and Definitions

Our articulated robot is represented using Featherstone’s recursive definition of an ar- ticulated body: an articulated body is composed of two other articulated bodies that are connected through aprincipal joint (See Fig 5.1) at a “handle”, the location on the link where the joint is connected [Fea99a].

At the base level, we have a set of n rigid bodies. For simplicity, we define our joint primitives to be 1-degree of freedom (DoF) revolute joints. More complex 2-DoF revolute joints can be added in a standard way as follows. A rigid body, bthin, with

zero (or sufficiently small) width is placed between each pair of bodies, bi and bi+1.

A standard 1-DoF joint will connect bi and bthin, and another 1-DoF joint rotated 90

degrees about the center of the chain will connect bi+1 and bthin.

We represent the articulated body’s state at time t as A(t) = {B,J,q(t),q(t)˙ }, where B = {b1, b2, . . . , b2n−1} is the set of rigid bodies, J = {j1, j2, . . . , j2n−2} is the

set of joints, q(t) is a vector of length 2n−2 of joint positions, and q(t) is a vector of˙ length 2n−2 of joint velocities at timet. To build the articulated body, pairs of bodies are connected via principal joints from the bottom up, following the recursive definition. The robot’s principal joint is then the middle joint along the linkage. This process forms anassembly tree (See Fig. 5.1).

Figure 5.1: Construction of an articulated body: An articulated body A is connected to body B at the principal joint, j2, to form body C. The assembly

tree for C is shown beneath the body. Forces and accelerations which govern C’s motion are shown.

For the forward dynamics problem, our goal is to compute the joint accelerations, ¨

q, given the state of the body and any external forces acting upon the body. This information is used to advance the simulation in time. The spatial acceleration of the body is given by:

         ˆ a1 ˆ a2 .. . ˆam          =          Φ1 Φ12 · · · Φ1m Φ21 Φ2 · · · Φ2m .. . ... . .. ... Φm1 Φm2 · · · Φm                   ˆ_f1 ˆ_f2 .. . ˆ fm          +          ˆ b1 ˆ b2 .. . ˆ bm          ,

whereˆai is the 6×1 spatial acceleration of linki,ˆfi is the 6×1 spatial force applied to linki, ˆbi is the 6×1 bias acceleration of link i(i.e. the acceleration linkiwould have if all link forces were zero), Φi is the 6×6 inverse articulated-body inertia of link i, and Φij is the 6×6 cross-coupling inverse inertia between links i and j.

is essentially an aggregate of linear and angular components of the physical quantity. For example, if ais the linear acceleration of a body, andα is the angular acceleration, then its spatial acceleration is ˆa=

   α a  

. Spatial algebra simplifies some of the notation, as

compared to the 3D vector formulation of these quantities. A detailed explanation of spatial notation and algebra is given in [Fea87].

With these motion equations, standard numerical integration techniques such as Euler’s method or various Runge-Kutta schemes can be used to determine the next state of the system. For complete dynamics simulation, it is necessary to determine how the body responds to interactions with the environment. Standard techniques use analytical constraints and impulse-based dynamics [Mir96, Kok04].

We can restate the path planning problem for articulated bodies as: Find a sequential set of robot configurations q(t0), ..,q(t1) such that noq(ti) intersects any obstacle in O andq(ti) satisfies the non-penetration, kinematic, and articulated dynamics constraints, whereq(t0) is the initial configuration of the robot, and q(t1) is the final configuration.