ixchel ramirez-alpizar - simulation-based optimal motion ......alpizar recently employed chomp for...

6
Simulation-based Optimal Motion Planning for Deformable Object* Eiichi Yoshida 1 , Ko Ayusawa 1 , Ixchel G. Ramirez-Alpizar 2 , Kensuke Harada 2 and Christian Duriez 3 Abstract— This paper presents a method for planning mo- tions of a flexible objects based on precise simulation using Finite Element Method (FEM), taking an example of ring- shape objects manipulated by robot arms, which is often seen in various applications. Since large deformation is implied, assembly planning with realistic simulation is important to ensure task efficiency for the robot and damage avoidance for the object. We first verify that the behavior of a ring- shape object by dual-arm manipulation is well predicted using FEM model of bent beam through a simulation along the path computed by optimization-based motion planning previously reported. Next, precise FEM model is integrated into trajectory optimization to compute a trajectory of robot hands minimizing the deformation energy as well as satisfying such criteria as collision avoidance and smoothness. Since the direct compu- tation leads huge computational cost, we present a realistic formula which transforms the planning problem into the static equilibrium problem of several FEM models located along the trajectory. Simulation results show that the proposed method is promising for such assembly tasks requiring large deformation. I. I NTRODUCTION Integration of dexterous robots has been accelerating in part and assembly manipulation in high-mix low-volume production system. Recently, dual-arm robots like Baxter [1] of Rethink Robotics and Nextage [2] of Kawada Industries have been introduced in cellular production systems by sharing the space with human workers. Especially, their dual- arm manipulation capacity combined with vision recognition system widens the range of tasks that those robot can execute [3]: picking up parts with complex shape and placing them in other assembled parts through dual-arm motions or regrasping. By delegating simple tasks to robots, humans can concentrate on more complex assembly tasks. The main tasks executed by those robots are currently limited to manipulations that do not involve large deforma- tions of parts. However in factory automation using industrial robots, there exist still some assembly tasks requiring large deformations that still need to be automated in manufactur- ing, such as cable handling, snapping assembly [4] or O-ring assembly [5]. In this paper, we focus on an assembly task to place a ring- shape flexible part on a desired position of a cylindrical part by bimanual manipulation (Fig. 1). This type of manipulation often appears in assembly of oil-seals and o-rings in chemical *This work was not supported by any organization 1,2 E. Yoshida and K. Ayusawa are with CNRS-AIST JRL (Joint Robotics Laboratory), UMI3218/RL, I .Ramirez-Alpizar, K. Harada xq are with Manipulation Research Group, both Intelligent Systems Research Institute, AIST, Tsukuba, Japan. E-mail: [email protected] 3 Christian Duriez is with INRIA Shacra Team, 40, avenue Halley, 59650 Villeneuve d’Ascq, France E-mail: [email protected] Fig. 1. Assembly of a ring-shape object including deformation or automobile industries. Previous research has addressed optimization-based motion planning of a ring-shape object in order to meet criteria of path smoothness, collision avoidance and minimum deformation simultaneously [5]. This work employs a simple model of the ring object by using a set of rigid bodies connected through springs. Likewise, although several methods have been proposed for motion planning of deformable objects [6], [7], [8], [9], [10], they reduce the dimension of the problem or utilize simplified model of deformable objects due to the complexity of the problem. This particular assembly implies necessarily large elastic expansion since we need to consider the space for the hands holding the object. Therefore manipulation motion without excessive deformation is important in order to avoid any damage on the object. Moreover, free deformation is assumed without taking into account the robot hands manipulating the object in previous research. This background motivates us to integrate precise modeling of deformable objects simulating their realistic behavior into assembly motion planning. In this paper we propose a more precise simulation model based on finite element method (FEM) for assembly motion planning of ring-shape objects, by explicitly including the manipulating robotic hands instead of allowing free object motions assumed in most of previous work. An optimization- based planning method is first described based on Covariant Hamiltonian Optimization and Motion Planning (CHOMP) method [11], [12] by combining the deformation energy into the cost. We then introduce a beam FEM model of ring-shape objects to demonstrate the validity of simulation- based motion planning that allows reducing excessive forces applied to the object during manipulation. Finally the FEM model is integrated into trajectory optimization problem together with mechanical equilibrium as an additional con- straint. An efficient method is presented for solving the complex planning as a static equilibrium problem through decoupling FEM models at each time instance of discretized trajectory. We will demonstrate simulation results to validate the effectiveness the proposed method.

Upload: others

Post on 04-Aug-2021

2 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Ixchel Ramirez-Alpizar - Simulation-based Optimal Motion ......Alpizar recently employed CHOMP for motion planning of deformable objects that is also a problem under severe constraints

Simulation-based Optimal Motion Planning for Deformable Object*

Eiichi Yoshida1, Ko Ayusawa1, Ixchel G. Ramirez-Alpizar2, Kensuke Harada2 and Christian Duriez3

Abstract— This paper presents a method for planning mo-tions of a flexible objects based on precise simulation usingFinite Element Method (FEM), taking an example of ring-shape objects manipulated by robot arms, which is often seenin various applications. Since large deformation is implied,assembly planning with realistic simulation is important toensure task efficiency for the robot and damage avoidancefor the object. We first verify that the behavior of a ring-shape object by dual-arm manipulation is well predicted usingFEM model of bent beam through a simulation along the pathcomputed by optimization-based motion planning previouslyreported. Next, precise FEM model is integrated into trajectoryoptimization to compute a trajectory of robot hands minimizingthe deformation energy as well as satisfying such criteria ascollision avoidance and smoothness. Since the direct compu-tation leads huge computational cost, we present a realisticformula which transforms the planning problem into the staticequilibrium problem of several FEM models located along thetrajectory. Simulation results show that the proposed method ispromising for such assembly tasks requiring large deformation.

I. INTRODUCTION

Integration of dexterous robots has been accelerating inpart and assembly manipulation in high-mix low-volumeproduction system. Recently, dual-arm robots like Baxter [1]of Rethink Robotics and Nextage [2] of Kawada Industrieshave been introduced in cellular production systems bysharing the space with human workers. Especially, their dual-arm manipulation capacity combined with vision recognitionsystem widens the range of tasks that those robot canexecute [3]: picking up parts with complex shape and placingthem in other assembled parts through dual-arm motions orregrasping. By delegating simple tasks to robots, humans canconcentrate on more complex assembly tasks.

The main tasks executed by those robots are currentlylimited to manipulations that do not involve large deforma-tions of parts. However in factory automation using industrialrobots, there exist still some assembly tasks requiring largedeformations that still need to be automated in manufactur-ing, such as cable handling, snapping assembly [4] or O-ringassembly [5].

In this paper, we focus on an assembly task to place a ring-shape flexible part on a desired position of a cylindrical partby bimanual manipulation (Fig. 1). This type of manipulationoften appears in assembly of oil-seals and o-rings in chemical

*This work was not supported by any organization1,2E. Yoshida and K. Ayusawa are with CNRS-AIST JRL (Joint Robotics

Laboratory), UMI3218/RL, I .Ramirez-Alpizar, K. Harada xq are withManipulation Research Group, both Intelligent Systems Research Institute,AIST, Tsukuba, Japan. E-mail: [email protected]

3Christian Duriez is with INRIA Shacra Team, 40, avenue Halley, 59650Villeneuve d’Ascq, France E-mail: [email protected]

Fig. 1. Assembly of a ring-shape object including deformation

or automobile industries. Previous research has addressedoptimization-based motion planning of a ring-shape object inorder to meet criteria of path smoothness, collision avoidanceand minimum deformation simultaneously [5]. This workemploys a simple model of the ring object by using a set ofrigid bodies connected through springs. Likewise, althoughseveral methods have been proposed for motion planningof deformable objects [6], [7], [8], [9], [10], they reducethe dimension of the problem or utilize simplified model ofdeformable objects due to the complexity of the problem.This particular assembly implies necessarily large elasticexpansion since we need to consider the space for the handsholding the object. Therefore manipulation motion withoutexcessive deformation is important in order to avoid anydamage on the object. Moreover, free deformation is assumedwithout taking into account the robot hands manipulating theobject in previous research. This background motivates us tointegrate precise modeling of deformable objects simulatingtheir realistic behavior into assembly motion planning.

In this paper we propose a more precise simulation modelbased on finite element method (FEM) for assembly motionplanning of ring-shape objects, by explicitly including themanipulating robotic hands instead of allowing free objectmotions assumed in most of previous work. An optimization-based planning method is first described based on CovariantHamiltonian Optimization and Motion Planning (CHOMP)method [11], [12] by combining the deformation energyinto the cost. We then introduce a beam FEM model ofring-shape objects to demonstrate the validity of simulation-based motion planning that allows reducing excessive forcesapplied to the object during manipulation. Finally the FEMmodel is integrated into trajectory optimization problemtogether with mechanical equilibrium as an additional con-straint. An efficient method is presented for solving thecomplex planning as a static equilibrium problem throughdecoupling FEM models at each time instance of discretizedtrajectory. We will demonstrate simulation results to validatethe effectiveness the proposed method.

Page 2: Ixchel Ramirez-Alpizar - Simulation-based Optimal Motion ......Alpizar recently employed CHOMP for motion planning of deformable objects that is also a problem under severe constraints

II. RELATED WORK

The problem of motion planning for deformable objecthas started attracting researchers’ interest in early 2000swhen randomized and sampling-based motion planning madea significant progress. Since deformable objects have farmore degrees of freedom (DOF) than even complex roboticmanipulators, the principal approach is to apply motionplanning method to its reduced model. Lamiraux and Kavrakiproposed a method for planning thin deformable objectsmanipulated by two actuators at the edges using sampling-based method in reduced dimensions with pre-computeddeformation configurations [6]. Mahoney et al. proposedanother approach to extract a reduced basis approximatingthe deformed configuration space using Principal ComponentAnalysis (PCA) [9].

Rather than converting the problem into a frameworkwhose reduced-dimension is subject to important simplifyingassumptions, some methods intrinsically rely on planningstrategies that were previously developed for rigid objectssuch based on the Probabilistic Roadmap Method (PRM)[13] or on the Rapidly-Exploring Random Tree (RRT) [14].Bayazit et al. [10] takes a two-stage PRM-based methodthat consists in first generating an “approximate path” whichmay contain collisions with the environment, and second,specifically computing deformations to adapt to such col-lisions. A more sophisticated method has been proposedlater by introducing a volume preservation constraint istaken into account in order to improve the physical realismof deformations computed based on a spring-mass systemrepresentation for efficient computation [7]. Rodriguez etal. proposed a planning method using RRT to deal withkinodynamic planning problem [8] for a deformable objectin an environment that is also flexible. Using an extensionof the mass-spring representation as a deformation model,a particular care is taken not to lead to unrealistic deforma-tions by considering so-called distance-preserving forces andvolume-preserving forces. A hierarchical approach is thenemployed for deformation computation. Another efficientsimplified representation for motion planning of deformableobjects using voxels has been proposed [15].

So far we have outlined related work based on sampling-based motion planning combined with models of deformableobject simplified to some extent. In general in those previousstudies, more attentions have been paid to implement aplanning method that exhibit realistically-looking behavior,rather than simulating the true reaction of the deformableobjects. Although some of the work like [8] introduced FEM-like method to model the deformation, most of them assumedeformable objects that can make free motions generated byforces applied at arbitrary points without considering robotichands manipulating the objects. One of the contributions ofthis paper is that we explicitly model the robot hands asDirichlet boundary conditions for FEM computation.

On the other hand, optimization-based motion planninghas also been studied more and more intensively. Amongthem, CHOMP [11], [12] has been proposed as a method

that refines the path using covariant gradient techniques witha given cost function to improve the quality of sampledtrajectories in a continuous manner. Since we can even startwith an initial path in collision, CHOMP is considered tobe suitable for highly constrained motion planning prob-lems including narrow passages. For this reason, Ramirez-Alpizar recently employed CHOMP for motion planningof deformable objects that is also a problem under severeconstraints by integrating deformation energy into the costfunction [5]. It is, however, based on simplified model ofdeformable objects. In the following sections we will presenta method with FEM model for deformation to simulate theprecise behavior in order to reduce the risk of damagesof manipulated objects and to improve the efficiency ofassembly.

III. SIMULATION-BASED PLANNING

In this section we integrate FEM simulation to plannedtrajectory to address such assembly tasks requiring largedeformation such as ring-shape objects. In our previous re-search [5] CHOMP was introduced to optimize the trajectoryξ connecting initial and goal configurations qi and qf (∈�m) of the robot hands manipulating the ring-shape object,by formulated by using an objective function as follows:

U(ξ) = wcFobs(ξ) + wsFsmooth(ξ) + weFenergy(ξ) (1)

where wc, ws, we, are the weights of objective for theobstacle Fobs, smoothness Fsmooth and energy Fenergy

respectively. The first two objectives are found in [12] andthe last one for energy is based on the deformation energy ofthe ring-shaped object based on simple model of rigid bodiesconnected by springs.

In order to confirm the feasibility of the planned trajec-tories, more realistic simulation model based on FEM hasbeen applied. We adopt SOFA (Software Open FrameworkArchitecture) [16] developed at mainly in INRIA, France.This framework has been developed principally for medicalsimulation based on real-time FEM computation, but itscapacity of simulating flexible objects allows other dynamicsimulations like soft robot control [17] or interacting de-formable objects that we are dealing with in this paper.

In this section we model the ring-shape object as a“bent” beam in FEM model with 40 nodes together with

�������������

Fig. 2. Grasping points of the ring-shape objects

Page 3: Ixchel Ramirez-Alpizar - Simulation-based Optimal Motion ......Alpizar recently employed CHOMP for motion planning of deformable objects that is also a problem under severe constraints

the collision model with the cylinder. We assume a two firmgrasping points by robotic hands as shown in Fig. 2 whichis modeled by Dirichlet boundary conditions for FEM. Asimple feedback control law is implemented to simulate themanipulations by robotic hands whose positions are given asxi(t) (i = 1, 2) with reference position xref

i (t) at time t.

f i = K(xi(t)− xrefi (t)) (2)

Here K is a diagonal matrix specifying the stiffness alongeach axis of motion. We have implemented a bent beam usingFEM model in SOFA and applied the control law.

The assembly task is to place the ring-shape object onthe rigid cylindrical part. The inner diameter of the ring is49mm with the cross-section of diameter 3.1mm whereas thediameter of cylinder is 50mm. The Young’s modulus of thering-shape object is 4.1 MPa and we used 3.0 × 1010N/mas the coefficient K in each axis. Figures 3 and 4 show thesimulation results with optimized and initial non-optimizedpath of each grasp point. The latter fails to place the ring-shape object correctly as it is stuck on the upper face of thecylinder. First, we can clearly observe that the ring-shapeobject is manipulated by the two firm grasping points. Theobject is pulled first horizontally then vertically to achievethe insertion. Then as can be seen, at the early stage (b)of failure case the object touches the cylinder which is notthe case for success case. While the successful manipulationthe vertical downward motion allows the whole ring-shapeobject to reach around the side surface, the right part of thering remain stuck on the upper face in the unsuccessful case.Thanks to efficient implementation of FEM model in SOFA,here the simulation was almost in real-time with a processorof Intel(R) Core(TM) i7-4900MQ CPU(2.80GHz). Althoughprecise parameter settings like friction still need to be refined,overall this simulation results demonstrate that precise object

modeling is useful to predict the behavior of flexible objectfor manipulation inducing large deformation.

Figure 5 shows simulated feedback input applied in verti-cal (z) direction based on control in Eq.(2) at the one of thegrasping points. The unit is omitted as the main purpose hereis to compare the two simulation results in Figs. 3 and 4. Inthis graph, we focus on the final state where the difference oftwo cases is clearly seen. Since the ring-shape object staysexpanded even after the manipulation trajectory is finished,the absolute value of force in case of failure case is largerthan the successful case. This leads to excessive residualstress to the manipulated object, which should be avoided.

In this section, we have shown that the validation ofplanned trajectory based on precise physical simulation usingFEM is meaningful for manipulation involving large defor-mation.

-15

-10

-5

0

5

3 4 5 6 7 8

Forc

e z

Time [s]

z1 successz1 fail

Fig. 5. Vertical force applied to ring-shape object

(a) Initial state (b) (c) (d) (e) (f) (g) Final state

Fig. 3. Snapshot of ring motion: successful case with optimized path

(a) Initial state (b) (c) (d) (e) (f) (g) Final state

Fig. 4. Snapshot of ring motion: failure case with non-optimized path

Page 4: Ixchel Ramirez-Alpizar - Simulation-based Optimal Motion ......Alpizar recently employed CHOMP for motion planning of deformable objects that is also a problem under severe constraints

IV. OPTIMIZATION-BASED PLANNING WITH FEMSIMULATION

This section presents the formulation of the optimization-based motion planning of a FEM object. The computationalcost of the optimization problem will be larger because of theenormous variables related to FEM computation. Therefore,we need to approximate problem Eq.(1) in order to acceleratethe computation.

Let us decompose trajectory ξ into discrete time samplesas follows:

ξ =[ξ(1)

T ξ(2)T · · · ξ(T )

T]

(3)

We now consider the following variables at each timeinstance.

ξ(t) =[d(t)

T x(t)T]T (4)

where, d represents the vector of all positions of each node ina FEM object, and x means the vector of all positions of thegrasping points by robotic hands. In this section, we considerthe following optimization problem instead of Eq.(1).

min U(ξ) subject to y(ξ) = 0 (5)

where, y represents the equality constraints related to FEMsimulation.

The stationary condition of problem (5) is as follows:(∂U∂ξ

)T

+

(∂y

∂ξ

)T

λ = 0 (6)

where λ is Lagrange multiplier. In order to optimize trajec-tory ξ, we solve Eq.(6), for example, by Newton-Raphsonmethod.

We now detail the each components in problem (5). Inthis paper, we consider the following boundary conditionsas constraint y(ξ):

yS � x(1) − xS = 0 (7)

yE � x(T ) − xE = 0 (8)

yf � df (t) − r(x(t)) = 0 (9)

where, xS and xE represents the initial and final positionsof the grasping points respectively, df is the vector of allpositions of constrained nodes, and r(x) means the forwardkinematics mapping of df with respect to x. The followingrelationship holds between df and d.

df = Sd (10)

where, S is the selection matrix which extracts df from d,.We now implement smoothness objective Fsmooth(ξ) as

follows:

Fsmooth(ξ) =

T∑t=2

l(t,t−1) +

T−1∑t=2

e(t) (11)

where,l(t,t−1) � ωl||x(t) − x(t−1)|| (12)

e(t) � ωe||x(t) − x(t)init||2 (13)

The first term in Eq.(11) represents the length of the wholetrajectory. The squared distance from the initial trajectoryx(t)

init is added in the second term for the computationalstability. ωl and ωe are the weighting factors.

In obstacle objective Fsmooth(ξ), we also consider thecollision condition between the FEM object and the otherenvironment. In this paper, we approximate the collisionconditions as the penalty functions against penetration.

Fobs(ξ) =∑t

Ep(t) �∑t

ωpEp(d(t)) (14)

where, Ep(d) represents the collision penalty function, andωp is its weighting factor. Let the energy function be imple-mented by

Fenergy(ξ) ≈∑t

Ee(t) �∑t

ωeEe(d(t)) (15)

where, Ee(d) is the elastic energy of the FEM object, andωe is its weighting factor.

In the original implementation of CHOMP, Fobs(ξ) isthe integrated penalty functions along trajectory ξ. In thispaper, we approximate both Fobs(ξ) and Fenergy(ξ) as thesummation of the individual corresponding functions. Withthe approximation, stationary condition (6) can be written asfollows:

yd(t) � g(d(t)) + STf (t) = 0 (16)

yx(t) � h(t) + J (t)Tf (t) + δt,1λS + δt,TλE = 0 (17)

where, f means the vector of the constraint forces acting onthe constrained nodes of the FEM objects, λS and λE areLagrange multipliers related to the initial condition, J � ∂r

∂x ,and δi,j is Kronecker’s delta symbol. g(t) and h(t) are theforces defined as follows:

g(d) �(∂Ee

∂d

)T

+

(∂Ep

∂d

)T

(18)

h(t) �(∂l(t−1,t)

∂d(t)

)T

+

(∂l(t,t+1)

∂d(t)

)T

+

(∂e(t)

∂d(t)

)T

(19)

It should be noted that g represents the standard staticequilibrium formula of the FEM object under the givenboundary conditions. Therefore, Eq.(16) is independent fromother time instances; on the other hand, Eq.(17) is depen-dent. When solving Eq.(16) and Eq.(17) by Newton-Rapthonmethod, we have to compute the Hessian matrix of theaugmented Lagrangian function of problem (5).

Let us redefine the trajectory including Lagrange multipli-ers as follows:

ξ̂ =[λS

T ξ̂(1)T ξ̂(2)

T · · · ξ̂(T )T λE

T]

(20)

where,ξ̂(t) =

[d(t)

T x(t)T f (t)

T]T (21)

We also concatenate all the stationary conditions and theconstraints as follows:

y =[yS

T y(1)T y(2)

T · · · y(T )T yE

T]

(22)

Page 5: Ixchel Ramirez-Alpizar - Simulation-based Optimal Motion ......Alpizar recently employed CHOMP for motion planning of deformable objects that is also a problem under severe constraints

y(t) =[yd(t)

T yx(t)T yf (t)

T]

(23)

The Hessian matrix can be written as follows:

Hδξ̂ = δy (24)

H �

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

O E O · · · O O OE H1 G1,2 · · · O O OO G2,1 H2 · · · O O O...

......

. . ....

... OO O O · · · HT−1 GT−1,T OO O O · · · GT,T−1 HT EO O O · · · O E O

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)where, E represents the identify matrix. Ht and Gt,s aredefined by

Ht =

⎡⎢⎢⎣

∂g(t)

∂d(t)

O ST

O∂h(t)

∂x(t)J (t)

T

S J (t) O

⎤⎥⎥⎦ (26)

Gt,s =

⎡⎢⎣O O O

O∂h(t)

∂d(s)

O

O O O

⎤⎥⎦ (27)

As can be seen from Eq. (25), all the columns and rowsrelated to the FEM variables are decoupled each other; thecolumns and rows related to the rigid-body variables at eachtime instance are coupled with those of the previous andthe following time instance. Since the number of variablesrelated to the rigid-body part is quite smaller than that of theFEM variables, the computational cost at each iteration ofthe Newton-Rapthon method is almost the same as when wesolve independent FEM equations at T time instances. In ad-dition, all the components in H and y can be obtained fromthe standard computation of FEM and rigid-body equationsrespectively. In other word, the motion-planning problemis transformed into the huge static equilibrium problem ofT objects whose grasping points are connected via virtualsprings along with the trajectory.

We tested the proposed optimization formula by using thering-shape object and the grasping points shown in Fig. 2,and the same environmental setting as that in section III.In this section, the ring-shape object was expressed withtetrahedral 3D meshes whose number of nodes is 1088 andnumber of elements is 3072, in order to test the proposedscheme for general FEM models. We also consider thealmost incompressibility condition of the ring object in thesimulation; the pressures inside the elements of the objectwere also taken into account by adding them in vector d asnew variables. We utilized FEM solver V-biomech [18] inorder to compute the terms of nonlinear FEM model used inthe optimization. The trajectory was discretized by T = 10samples, and the initial trajectory before optimization wasgiven as shown in Fig. 6. Since the initial trajectory wasjust interpolated linearly without considering deformation,

the ring was penetrated inside the cylinder during the motion.The method was implemented on the work station with thesame CPU used in seciton III. The total computation timeof the whole optimization was around 30 minutes.

Fig. 7 shows the obtained trajectory after the optimization,It shows that another strategy to archive the insertion fromsection III. The left handle point in the figure remained fixedon the top of the cylinder during first time samples, andthe right handle point went down and close to the side ofcylinder, which leads the increase of the diameter of the longaxis along the vertical direction to this paper. After hookingthe half part of the ring on the cylinder, the left handle pointpassed over the cylinder without collision, and went downto the final position to achieve the insertion.

Though the pilot optimization framework can obtain thelocal optimal solution according to the objective functions, itdoes not guarantee the global optimal solution. Tor this pur-pose we need another framework to explorer the candidate ofthe trajectory in the initial stage, for example, by the simplesimulation as shown in section III. The current optimizationalso approximates the integration of the objectives along withthe trajectory by the simple summation. Those issues will beinvestigated in our future works.

V. CONCLUSION AND FUTURE WORK

This paper presented a simulation-based optimal motionplanning method for assembly of a flexible ring-shapeobject including large deformation. After introducing anoptimization-based motion planning called CHOMP that canintegrate objective functions of minimum energy manipula-tion, realistic simulation model based on FEM, together withthe collision detection, was applied to verify the plannedtrajectory. The ring-shape object was represented by a bentbeam firmly grasped by robotic hands of a dual-arm manipu-lator. Comparing the motion of optimized and non-optimizedinitial trajectories, we could recognize the difference ofinput force applied to the object, which leads to excessivemanipulation force. This simulation showed the significanceof realistic simulations that can predict the precise behaviorsof the object. Next we presented optimization-based motionplanning integrating the more accurate FEM model contain-ing more than 1000 nodes by using objective functions ofsmoothness, collision avoidance and minimum deformationenergy under static equilibrium conditions. An efficient com-putation scheme was derived to solve the planning problemas a static equilibrium problem of decoupled FEM objects ateach time instance in a discretized model of the optimizationproblem. Simulation results demonstrated that the proposedmethod can provide plausible behaviors of deformable ring-shape object to plan the path for the robotic hands.

Future work includes the refinement of the proposedoptimization-based method. As we have discretized the tra-jectory and used the sum of objective function instead ofintegration along the trajectory, it is the next step to address.Global optimization scheme with even more efficient FEMcomputation will be also forthcoming stage of this research.The verification of the planned path with experiments with a

Page 6: Ixchel Ramirez-Alpizar - Simulation-based Optimal Motion ......Alpizar recently employed CHOMP for motion planning of deformable objects that is also a problem under severe constraints

1

6

2

7

3

8

4

9

5

1010

1

Fig. 6. Snapshot of initial trajectory of ring motion before optimization

1

6

2

7

3

8

4

9

5

10

Fig. 7. Snapshot of obtained trajectory of ring motion after optimization

real robot is also another future topic. We intend to confirmthe predicted behavior from the simulation and to improvethe simulation model based on the experimental results.

ACKNOWLEDGMENTS

We would like to express sincere gratitude to Mr. Tu-HoaPham, currently a Ph.D candidate at CNRS-AIST JRL, whoperformed a comprehensive survey of related work on motionplanning of deformable objects.

REFERENCES

[1] http://www.rethinkrobotics.com/baxter/, 2015.[2] http://nextage.kawada.jp/, 2015.[3] http://www.glory.co.jp/company/news/2012/1011.html, (in Japanease).[4] J. Rojas, K. Harada, H. Onda, N. Yamanobe, E. Yoshida, K. Nagata,

and Y. Kawai, “Towards snap sensing,” Int. J. Mechatronics andAutomation, vol. 3, no. 2, pp. 69–93, 2013.

[5] I. G. Ramirez-Alpizar, K. Harada, and E. Yoshida, “Motion planningfor dual-arm assembly of ring-shaped elastic objects,” in Proc. 2014IEEE-RAS Int. Conf. on Humanoid Robots, 2014, pp. 594–600.

[6] F. Lamiraux and L. E. Kavraki, “Planning paths for elastic objectsunder manipulation constraints,” Int. J. Robotics Research, vol. 20,no. 3, pp. 188–208, 2001.

[7] R. Gayle, M. C. Lin, and D. Manocha, “Constraint-based motionplanning of deformable robots,” in Proc. 2005 IEEE Int. Conf. onRobotics and Automation, 2005, pp. 1046–1053.

[8] S. Rodriguez, J.-M. Lien, and N. M. Amato, “Planning motion incompletely deformable environments,” in Proc. 2006 IEEE Int. Conf.on Robotics and Automation, 2006, pp. 2466–2471.

[9] A. Mahoney, J. Bross, and D. Johnson, “Deformable robot motionplanning in a reduced-dimension configuration space,” in Proc. 2007IEEE Int. Conf. on Robotics and Automation, 2010, pp. 5133–5188.

[10] B. Bayazit, J.-M. Lien, and N. M. Amato, “Probabilistic roadmapmotion planning for deformable objects,” in Proc. 2002 IEEE Int.Conf. on Robotics and Automation, 2002, pp. 2126–2133.

[11] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “CHOMP:Gradient optimization techniques for efficient motion planning,” inProc. 2009 IEEE Int. Conf. on Roboics and Automation, 2009, pp.489–494.

[12] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith,C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa, “CHOMP: Covari-ant hamiltonian optimization for motion planning,” Int. J. RoboticsResearch, vol. 32, no. 9-10, pp. 1164–1193, 2013.

[13] L. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars, “Probabilisticroadmaps for path planning in high-dimensional configuration spaces,”IEEE Trans. on Robotics and Automation, vol. 12, no. 4, pp. 566–580,1996.

[14] S. LaValle and J. Kuffner, “Rapidly-exploring random trees: Progressand prospects,” in Algorithmic and Computational Robotics: NewDirections, K. M. Lynch and D. Rus, Eds. A K Peters, 2001, pp.293–308.

[15] C. Phillips-Grafflin and D. Berenson, “Representation of deformableobjects for motion planning with no physical simulation,” in Proc.2014 IEEE Int. Conf. on Robotics and Automation, 2014, pp. 98–105.

[16] http://www.sofa-framework.org/.[17] C. Duriez, “Control of elastic soft robots based on real-time finite

element method,” in Proc. 2013 IEEE Int. Conf. on Robotics andAutomation, 2013, pp. 3982–3987.

[18] J.L.Alves, N.Yamamura, T.Oda, and C.Teodosiu, “Numerical simula-tion of musculo-skeletal systems by V-biomech,” 2010.