IROS2019国际学术会议论文集 0753

Towards Reversible Dynamic Movement Primitives I igo Iturrate1 2 Christoffer Sloth1 Alja Kramberger1 Henrik Gordon Petersen1 Esben Hallundb k stergaard2 and Thiusius Rajeeth Savarimuthu1 1Maersk McKinney Moller Institute University of Southern Denmark Odense Denmark inju chsl alk hgp trs mmmi sdu dk 2Universal Robots A S Odense Denmark iit esben universal Abstract In this paper we present an initial approach towards reversible robot movement primitives Our approach is a modifi cation of Dynamic Movement Primitives DMPs a widely used framework for robot learning from demonstration DMPs are based on dynamical systems to guarantee properties such as convergence to a goal state robustness to perturbation and the ability to generalize to other goal states Yet a main limitation of their original formulation is that they do not allow for movements to be reversed Thus to execute the same task forwards and backwards would mean to learn two separate primitives We propose to replace the transformation system in DMPs with the Logistic Differential Equation LDE a known time reversible non linear system Similarly to the original DMP formulation our system s temporal evolution is controlled by a phase system which in our case is derived from the LDE to guarantee reversibility We evaluate our approach experimentally with demonstration data from a real robot assembly task and show comparable properties to those of the original DMP system I INTRODUCTION The past decade has seen a growing trend towards robot automation in industrial manufacturing in order to lower production costs At the same time the number of Small and Medium Enterprises offering customized small batch production has risen Both of these trends have been enabled by the adoption of cheap and easy to use automation solu tions particularly in the form of collaborative robots These robots make use of intuitive graphical user interfaces and hand guided via point teaching to allow non expert users to quickly develop robot applications While collaborative robots have lowered the entry barrier for industrial automation there is a strong push for methods that are even more user friendly One such method is pro gramming by demonstration by which a user shows the robot how to perform a task either by performing it themselves by guiding the robot kinesthetically 1 or by other methods 2 and the robot learns the corresponding skill Within programming by demonstration Dynamic Move ment Primitives DMPs 3 is one of the most popular frameworks for learning skills DMPs can be learned from a single demonstration as opposed to the larger datasets required by statistical methods This has practical value in cases where quick task setup is important such as industrial applications with small batch sizes and quick changeover requirements With a basis in the neuroscientifi c domain 4 initial work with DMPs in robotics focused on imitation learning 5 3 6 and was later utilized in several other robotic domains such as humanoid bipedal locomotion 7 adaptive frequency modulation for periodic movements 8 9 reinforcement learning of task parameters 10 11 and generation of motor skill libraries 12 13 14 for automatic trajectory generation One of the advantages of this framework is that movement primitives can be easily generalized to new goal confi gu rations 15 and the speed of execution can be changed Furthermore because their formulation is grounded in dy namical systems theory DMPs can be extended to exhibit more complex behaviors such as obstacle avoidance 16 or hybrid position force control 17 All of these characteristics make them versatile and applicable in a variety of contexts and tasks In recent work the area of robotic assembly has gained attention Here the goal is to not only consider the kinematic trajectory but also the measured forces and torques arising during task execution 18 to adapt the trajectory to fulfi ll the task requirements For safe interaction with the environment force feedback can directly be coupled within the DMP on the velocity and acceleration levels 19 When incorporating compliant coupling terms the overall stability of the system must be considered 20 to ensure a safe execution Within industrial assembly reversibility of the motions is also of general interest In a fi rst use case a reversible motion representation could be used to both assemble and disassem ble a workpiece or palletize and depalletize Perhaps more importantly reversibility has proven to be an effective error recovery strategy for a high percentage of tasks 21 22 Here if the robot contact forces exceed the nominal values expected for the task the robot can backtrack to a previous point in the assembly trajectory and try again Because of the inherent sensor uncertainty in the assembly of low tolerance parts this retrial strategy will often succeed 21 Dynamic Movement Primitives are unable to reverse the trajectory because the second order spring damper system becomes unstable in the backwards direction This means that it is necessary to learn separate primitives for the forwards and backwards directions To the extent of our knowledge this issue has only been discussed by Nemec et al 23 In their work the user is able to incrementally teach a correction term that scales the demonstration trajectory in time including the possibility of reversing it However to 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE5063 allow backwards execution the authors switch to a second DMP learned from the backwards trajectory in order to avoid instability In this paper we present a reformulation of Dynamic Movement Primitives that uses the time symmetrical prop erties of the Logistic Differential Equation as a basis for constructing a reversible DMP system In doing so we attempt to preserve the main properties of traditional DMPs This manuscript is structured as follows First we review the original formulation of DMPs and formalize their main properties as suggested in the literature in section II We then formalize the requirements for our defi nition of reversibility in section III Upon this we present our suggested approach towards constructing reversible DMPs from the Logistic Differential Equation in section IV Section V then analyzes our system with respect to reversibility and stability and the properties of DMPs Our method is then evaluated experi mentally in both simulation and on a real robot in section VI followed by a discussion on its strengths and limitations in section VII We conclude this work in section VIII II DYNAMICMOVEMENTPRIMITIVES Dynamic Movement Primitives are mathematically for malized as a set of transformation systems 1 one per output dimension which control the evolution of the trajectory and a canonical system 2 which controls temporal evolution of the system and provides coupling between dimensions 2 y y y g y y f x 1 x xx 2 where g represents the goal state or attractor of the system y y x R are positive gain constants and R is a time constant The term f x called the forcing term in 1 is typically defi ned as follows f x PN i 1 i x wi PN i 1 i x x g y0 3 i x exp x ci 2 2 2 4 where N Z is the number of Gaussian basis functions i 4 with centers ci variance and weights wi The constant y0in 3 corresponds to the initial state of the transformation system 1 If the parameters are selected as 0 y 4 0 the system is critically damped and has a unique point attractor at y g Notice that excluding the f x 1 corresponds to a second order spring damper system Term f x introduces non linear dynamics to the system and can be learned to gen erate any arbitrary smooth desired trajectory Furthermore the system 1 is not explicitly dependent on time but is instead dependent on a phase variable x Because x appears in 3 it will eventually gate out the infl uence of f x in 1 as it converges to zero thus guaranteeing convergence of the transformation system to its attractor state g Since f x is linear in its parameters wi learning a desired trajectory can be done by linear regression In this case given a demonstration trajectory ydemo ydemo ydemo a desired forcing term that results in the demonstration movement can be calculated according to fdesired 2 ydemo y y g ydemo ydemo 5 From 5 a least squares weighted linear regression prob lem can be formulated in order to approximate fdesiredusing f as defi ned in 3 Details on the parameter learning can be found in 3 III REQUIREMENTS FOR THEREVERSIBILITY OFDMPS In order to formulate a reversible DMP we must fi rst defi ne a set of design requirements needed to obtain the desired behavior First of all a DMP must be globally asymptotically stable The basic minimum requirement for it to be globally stable in both the forwards and backwards directions is that the underlying dynamical system must have two critical points one at the goal state which must be an attractor in the forwards direction and another at the start state which must be an attractor in the backwards direction Furthermore it would be preferable if the underlying dynamics of the transformation system were reversible This would mean that the system s trajectory would be identical in both directions allowing us to use the same forcing term for backwards execution as used for forwards execution A dynamical system is reversible if there exists an involution in phase space which reverses the direction of time 24 Specifi cally let x Rnand f Rn Rn then dx dt f x 6 is reversible if there exists a map G Rn Rnwhere G G Id such that dG x dt f G x 7 Notice that according to this defi nition the existence of an attractor in a dynamical system implies the existence of a repeller in the corresponding reverse time system It is therefore impossible to have global stability and reversibility simultaneously Instead either of the requirements must be relaxed A The Logistic Differential Equation We now consider a dynamical system which fulfi lls the re quirements of having two critical points and being reversible as per 6 and 7 An example of such a system is the logistic differential equation LDE 25 y y a b y 8 which is reversible since the involution G y 7 a b y reverses the direction of time for the system where a 6 b R As previously stated this system has an attractor at the goal b and a repeller at the starting state a for the forwards system and the points switch roles for the reverse system 5064 This implies that the system is not globally asymptotically stable a stable alternative will be introduced in section III B Consider the logistic differential equation 8 and let a b then the region of attraction of y b is a R and likewise for the reverse system the region of attraction of y a is b R We can now re write the system 8 in a form more similar to that of DMPs 1 and 2 with a goal state g and start state y0 and with positive gain R as follows y g y y y0 9 However this parametrization based on is not ideal because we would like to be able to modulate speed and direction of execution simultaneously and smoothly With smooth modulation of speed would imply a discontinuity in as y 0 when With this in mind we redefi ne the system as y fLDE y g y y y0 10 where R can be interpreted as 1 i e a speed scaling instead of time scaling constant such that positive values of move the system forward negative values move it backward and a value of zero halts it From now on and for the rest of this paper we will assume y0 g The regions of stability of 10 can then be written as Rforward y R 0 y g 1 y0 11 Rreverse y R 1 y g 1 y0 12 for the forwards and backwards systems respectively A plot of the function 10 can be seen in Fig 1 101234567 3 2 1 0 1 2 3 Fig 1 Graph of the generalized logistic equation 10 with g 5 and y0 1 Notice that the system is time reversible i e yreverse yforwardand yreverse yforward B The Stabilized Logistic Differential Equation In order to satisfy the requirements that a DMP should be globally asymptotically stable towards the attractor g we propose to stabilize the LDE by reversing the sign of y in the unstable region Thus we defi ne the Stabilized LDE SLDE system piecewise as follows yforward fLDE y if y Rforward fLDE y otherwise 13 yreverse fLDE y if y Rreverse fLDE y otherwise 14 fSLDE y yforwardif 0 yreverseotherwise 15 Note that the system is now globally stable However stabilizing the system has in turn resulted in limiting its reversibility such that it is only reversible for y y0 g as shown in Fig 2 As stated earlier this compromise is nec essary if global stability and reversibility are simultaneously needed as reversibility precludes global stability 101234567 3 2 1 0 1 2 3 Fig 2 Graph of the stabilized generalized logistic equation 10 with g 5 and y0 1 Notice that the system is now only time reversible for y y0 g IV REVERSIBLEDYNAMICMOVEMENTPRIMITIVES With the requirements and considerations regarding re versibility established in section III as a basis we now reformulate DMPs using the SLDE Based on 15 we reformulate the transformation system y fSLDE y p 16 p f z 17 The new phase system renamed as z to account for the different dynamics is given by a logistic differential equation with y0 1 and g 1 z z 1 z z 1 18 where gain constant z R controls the speed of conver gence The new forcing term f z is defi ned as follows f z PN i 1 i z wi PN i 1 i z v z g y0 19 The weighted Gaussian basis functions are defi ned as in 4 but as a function of z instead of x We introduce a separate gating term given by v 1 1 R as v z 1 z2m 1 2m 20 5065 with m R Note that this choice of gating system solves a drawback of the term defi ned in 2 namely that the weights wiof the forcing term will take large values towards the end of the trajectory due to the gating term s convergence to zero An alternative gating system that also solves the scaling problem of wihas been presented in 26 V SYSTEMANALYSIS A Partial vs Complete Reversibility As mentioned in section III B making the system globally stable limits its reversibility to the region y y0 g Outside this the dynamics are not reversible Due to this a compromise must be reached depending on the properties desired from the system Notably there are in theory two main alternatives with our system 1 If the system must be globally stable a separate forcing term must be learned for the forwards and reverse trajectories to account for the non reversibility in regions outside of y y0 g Thus 17 is redefi ned as p fforward z if 0 freverse z otherwise 21 where fforward z and freverse z are forcing terms de scribed according to 19 and trained on the forwards and backwards executions of the demonstration trajec tory respectively 2 If global stability is not a concern or if the system always operates within y y0 g the non stabilized generalized logistic equation in 10 can be used as the foundation of the transformation system Note that perturbing the system beyond g or y0could be po tentially dangerous as it would then become unstable with fi nite escape time We therefore do not consider this a practical solution and will not discuss it for the remainder of this work B Properties with Respect to Dynamic Movement Primitives As formulated by Ijspeert et al 3 the design of a point to point discrete i e non rhythmic DMP is based on a series of design principles that defi ne the properties that the system should follow Thus when formulating Reversible DMPs we took these principles into consideration We now briefl y outline how our approach relates to this The original principles of DMPs 3 are stated in bold and then followed by their equivalent in the Reversible DMP system 1 Guaranteed congergence to a point attractor the goal g Reversible DMPs have a stable global attractor at the goal g However in order for the system to be reversible the initial state y0must also be a critical point of the system see section III as the points will switch role in the reversed system In our stabilized LDE based system due to the switched dynamics y0 is a saddle point This is theoretically problematic because the system s evolution would halt if it were to reach this point with zero velocity and because initially it would never be able to escape the start state In practice the forcing term will push the system away from y0 such that the above will not be an issue without any modifi cation to the target trajectory path Furthermore in a real computer system residuals from fl oating point approximation will make it very unlikely that the system will get stuck in the saddle point 2 The system should be autonomous without explicit time dependence and allow for time modulation i e speed up or slow down Since the generalized stabilized LDE on which our system is based is au tonomous and the rest of the terms forcing term and phase are formulated in a way that is analogous to the original DMP system and thus also autonomous this property is retained 3 Multi dimensional dynamical systems need to be coordinated in a stable way Similarly to the original DMP formulation 2 this is done through the phase system 18 4 Open parameters should be easy to learn in order to reproduce the desired movement i e the model should be linear in the parameters Our parame terization of the new forcing term 19 is analogous to the original 3 and thus linear in the open parameters Locally Weighted Linear Regression can still be used Learning consists of computing pdesired ydemo fSLDE ydemo 1 22 for the forwards trajectory Likewise by inverting ydemo and ydemoin time and setting 1 the backwards forcing term can be learned 5 The system needs to be able to incorporate cou pling terms This is an implicit property of dynamical systems and is therefore retained 6 The system should allow real time computation and modulation of control parameters for online trajec tory modulation i e changes in the parameters g y0and in the case of reversible DMPs The reversible DMP system retains this property as wil