IROS2019国际学术会议论文集 1717

Exploiting linearity in dynamics solvers for the design of composable robotic manipulation architectures Sven Schneider a band Herman Bruyninckxb c Abstract We investigate two major limiting factors in the design and implementation of modern dynamics solvers that interfere with their full utilization in versatile manipulation driven robotic software architectures The fi rst limitation orig inates from the design of those solvers which aims at compu tational effi ciency while neglecting composability Instead we advocate to design the solvers in such a way that they exploit linearity in the equations of motion to fully decompose the state of a kinematic chain This enables a versatile recomposition and more fl exible applications Secondly we have observed that most implementations follow the programming principle of information hiding Consequently the internal state that is used to compute motion control commands is withheld from other parts of the software architecture We tackle this problem by following a datafl ow programming paradigm and separating the software s datafl ow from the control fl ow Thereafter we demonstrate those two simple yet effective strategies to overcome the limitations along various case studies I INTRODUCTION As modern service robots such as KMR iiwa1 Care O bot 42or Atlas3are getting more prevalent they are supposed to solve tasks of increasing diffi culty that especially require physical interaction with the environment Examples of those tasks include handling objects via grasping pushing or placing tool usage or shoring to ensure proper balancing in case of humanoid robots Solving such tasks demands for elaborate hardware in conjunction with sophisticated software As a result robotics is an interdisciplinary fi eld of research where the individual complex domains dynamics control vision or automated planning to name a few have often developed solutions independently of robotics problems Consequently robotics is often seen as merely an integration problem However this leads to a compartmen talization where each domain tries to solve robotics with the tools available at its hand mostly by improving vision planning or control algorithms in increasingly specialized scenarios In embedded systems similar problems have been encoun tered when hardware and software were developed inde pendently which led to incompatibilities A solution that has evolved from those problems is the hardware software codesign 1 which means that both the hardware and the Corresponding author sven schneider h brs de a Dept of Computer Science Bonn Rhein Sieg University of Applied Sciences Germany b Dept of Mechanical Engineering KU Leuven Belgium c Dept of Mechanical Engineering TU e Eindhoven Netherlands 1 mobile robot systems kmr iiwa 2https www care o bot de en care o bot 4 html 3 software is concurrently derived from and designed upon common requirements Consequently the benefi ts of hard ware and software can be exploited while avoiding their drawbacks A similar approach in robotics may be called hardware software knowledge codesign with the goal that also knowledge including data or information is shared between domains so that they can cooperate to solve robotic problems by i relying on and exploiting other domains to improve their domain specifi c robotic solutions and ii con tributing to other domains so that those can in turn improve their solutions One concrete example can be seen in the passive compliance of soft hands 2 that allows to transfer inherent task complexity from the control system to the hardware Frequently the resulting system architecture is then no longer a deep and static feed forward pipeline but instead forms a dynamic graph of interacting components To follow such an approach it is important that the domain specifi c solutions are composable By composability we refer to a sub system s property of being employable in a wider context of the overall system without i requiring any internal changes to that sub system and ii without inter fering with the semantics of that sub system Separation of concerns complements composability by fi rst decomposing the overall system along various design dimensions In this paper we address the problem of a lack of separation of concerns in dynamics solvers and demonstrate how a holistic approach looks like in robotic manipulation by introducing few simple changes Our contributions are three fold We show how linearity in the equations of motions can be exploited to foster composability in manipulation architectures We show how separation of concerns in terms of datafl ow and control fl ow supports that composability We provide multiple case studies to substantiate the previous two points The remainder of the paper is organized as follows Firstly we provide a short overview of dynamics solvers in the context of robotic control loops Afterwards we refactor those solvers so that they are composable with other elements of the architecture and demonstrate the benefi ts in various use cases Finally we discuss those results in the context of related work II PRELIMINARIES A Acceleration constrained hybrid dynamics We start with a brief overview of effi cient domain specifi c solvers for the equations of motion in the domain of 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 IEEE7433 kinematic chains For more details we refer the reader to 3 Those solvers rely on a recursive formulation of the equations of motion to compute requested quantities e g Cartesian poses and twists or joint torques from given quantities e g joint positions and velocities or Cartesian forces The building blocks of those solvers are sweeps i e primitive recursions over a kinematic chain during which physical quantities are transformed propagated or accumulated A sweep can traverse the kinematic chain in the outward direction from proximal to distal segments or in the inward direction from distal to proximal segments The Popov Vereshchagin solver 4 in Algorithm 1 is an acceleration constrained hybrid dynamics ACHD solver Hybrid dynamics means that during the computations of the motion also the forces required to realize those motions are determined as a by product The motion of a kinematic chain can be subject to various constraints which we also call motion drivers because they allow to impose purposeful behaviour on a manipulator The ACHD solver supports three types of motion drivers Feed forward torques ffthat act on the joints and can originate from a variety of infl uences e g real forces like joint friction or virtual forces to command a joint away from joint limits or singularities External forces Fextto represent Cartesian force con straints on each segment for instance humans pushing the kinematic chain but also virtual inputs to compli antly move the kinematic chain as specifi ed in a task Acceleration constraints on the end effector Fcstr N and Ecstr N which can describe for example rigid real world obstacles but also desired virtual accelera tions that originate from a task specifi cation Fcstr N specifi es the active Cartesian directions of the con straint as a matrix of unit forces whereas Ecstr Nare the setpoints measured in the unit of acceleration energy a product of force and acceleration that does not have a commonly established name in classical mechanics and originates from Gauss principle of least constraint 5 Fcstr Nis a 6 NC matrix where NC represents the number of acceleration constraints that are imposed on the end effector Underconstrained motion specifi ca tions are naturally resolved using Gauss principle of least constraint Since a derivation of the ACHD solver is out of scope for this paper we only provide a brief overview Algorithm 1 shows the equations of the solver in body coordinates i e following the frame assignment in 3 As a recursive formulation of the equations of motion for a serial kinematic chain with segments numbered from 0 to N each sweep relates quantities in two adjacent bodies where i and i 1 indicate the distal and proximal body respectively Those two bodies are connected by joint i The kinematic chain model contains the joint description including the motion subspace matrix S and its dual ST the joint location and rigid body inertia M Given the motion drivers as well as the current joint position q and joint Algorithm 1 Original Popov Vereshchagin solver Input Chain model q q ff Fext Fcstr N Ecstr N 1sweep Outward 2 Xi iXi 1 Xi 1 Si qi 3 Xb i Xi Si qi 4Fb i Xi Mi Xi Fext i 5sweep Inward 6Di di ST iM A iSi 7P T i 1 MA iSiD 1 i ST i 8MA i 1 Mi 1 i 1XT i P T iM A i i 1Xi 9F A b i 1 Fb i 1 i 1XT i h P T i F A b i M A i Xb i MA iSiD 1 i ff i i 10F A cstr i 1 i 1XT iP T iF A cstr i 11EA conf i 1 E A conf i F A cstr i T n Xb i SiD 1 i h ff i ST i F A b i M A i Xb i io 12EA cstr i 1 E A cstr i F A cstr i TSiD 1 i ST iF A cstr i 13 EA cstr 0 1 h F A cstr 0 T X0 EA conf 0 Ecstr N i 14sweep Solver 15 ctrl i ST iF A cstr i 16 qi D 1 i ff i ctrl i D 1 i n ST i h F A b i M A i iXi 1 Xi 1 Xb i io 17 Xi iXi 1 Xi 1 Si qi Xb i velocity q the solver computes the following state variables for each body and its associated joint The Cartesian motion consists of i motion and force transforms X and XTw r t body i 1 ii velocity X where X0 0 and iii velocity dependent bias acceleration Xbas well as resultant segment accelera tion X Gravity is taken into account by initializing the acceleration at the base with the negative gravity vector i e X0 0 g in angular before linear order The articulated state that the body feels due to being part of a kinematic chain is represented by inertia MA bias force F A b constraint force F A cstr acceleration en ergy due to the constraint force EA cstr and acceleration energy due to the confi guration of the body EA conf The respective initial values are MA N MN F A b N Fb N F A cstr N Fcstr N EA cstr N 0 EA conf N F T cstr N X0 The joint variables are the resultant joint acceleration q and the torque ctrlto realize that acceleration They are related by the overall inertia D around the joint axis which also includes the motor s rotor inertia d The projection operator P T acts on forces and inertias At the base the Lagrange multiplier is computed as the magnitude of the constraint forces The product of that Lagrange multiplier and the articulated unit constraint forces determines the overall constraint force that each body feels due to the end effector s acceleration constraint We note that the equations until line 9 are exactly the same as those in the better known Articulated Body Algorithm ABA by Featherstone 3 to compute the forward dynamics 7434 Fig 1 Example of a typical control loop in a robotic system for tracking a trajectory The instantaneous solver red elements is separated from the non instantaneous controller green elements of a kinematic chain In the outward solver sweep the only difference to the ABA is the addition of the end effector constraint torques see line 15 to the overall joint torques in line 16 Thus the ACHD solver is an extension of the ABA B Dynamics and control In optimization based control frameworks e g the Stack of Tasks 6 typically the objective is to minimize a task error while physical consistency enters as a constraint The ACHD algorithm also solves an optimization problem a linear quadratic problem derived from Gauss principle of least constraint However the ACHD solver s objective is to compute those motions that are closest in terms of acceleration energy to the unconstrained motions Any deviation e g a control task is a constraint composed on top of the natural dynamics Thus from an architectural point of view the ACHD solver fosters a clean separation of concerns into instantaneous dynamics and non instantaneous control Fig 1 depicts the ACHD solver in a simple example appli cation for tracking an end effector trajectory that is specifi ed by a desired Cartesian motion via Xd Xdand Xd In this example the solver s feed forward torque and external force interfaces are not used However the application could easily be composed with further controllers around those interfaces The forward position kinematics FPK and forward velocity kinematics FVK compute the current end effector s pose Xmand velocity Xmfrom the measured joint position and velocity Here the robot s torque interface is employed to achieve the specifi ed motion task whereas the computed joint accelerations are not used We will revisit this example in the next section to demonstrate our approach III APPROACH A Linearity in solvers As we have outlined in the previous section domain specifi c solvers compute various intermediate state variables that have physical meaning and many of which can also be measured by sensors All too often the major goal when designing and implementing solvers is computational effi ciency This is obviously achieved by reducing the number of operations to compute the result especially by composing quantities as early as possible In Algorithm 1 this is evident Algorithm 2 Decomposition sweeps Input Chain model q q ff Fext Fcstr N Ecstr N 1sweep Outward 2 Xi iXi 1 Xi 1 Si qi 3 Xb i Xi Si qi 4Fb i Xi Mi Xi 5sweep Inward 6Di di ST iM A iSi 7P T i 1 MA iSiD 1 i ST i 8MA i 1 Mi 1 i 1XT i P T iM A i i 1Xi 9F A b i 1 Fb i 1 i 1XT iP T i F A b i M A i Xb i 10F A i 1 i 1XT i PT iF A i M A iSiD 1 i ff i 11F A ext i 1 Fext i 1 i 1XT iP T iF A ext i 12F A cstr i 1 i 1XT iP T iF A cstr i 13EA b i 1 E A b i F A cstr i T h Xb i SiD 1 i ST i F A b i M A i Xb i i 14EA i 1 E A i F A cstr i TSiD 1 i ff i STiF A i 15EA ext i 1 E A ext i F A cstr i TSiD 1 i ST iF A ext i 16EA cstr i 1 E A cstr i F A cstr i TSiD 1 i ST iF A cstr i in line 4 composition of inertial force and external force to bias force as well as lines 9 and 11 composition of inertial contributions and all motion drivers beside end effector constraints However as a result of this approach the solvers are now narrowly focused on motion control tasks and lack composability with further components in the robot s software architecture a Decomposition sweeps The insight underlying our approach is that dynamics solvers are completely linear without requiring approximations albeit kinematic chains being non linear systems We exploit this observation to fully decompose the state along all the motion drivers in the fi rst two sweeps hence the name decomposition sweeps as shown in Algorithm 2 The transformation from Algorithm 1 to Algorithm 2 is a straight forward decomposition of the bias force and confi guration dependent acceleration energy in the original formulation that purely relies on linear algebra The articulated constraint force and its associated acceler ation energy remain as in the previous formulation However there are two new articulated forces F A and F A ext with their associated articulated acceleration energies EA and EA ext for the feed forward torque and external force motion drivers respectively Additionally the bias terms F A b and EA b only track the velocity dependent inertial contributions Further it is evident from the equations that gravity is not considered as a separate force but has to be added via the solver sweep like in the original formulation of the ACHD The reason for this design decision is that gravity is independent of the tasks and therefore has to be taken into account as is anyway Nevertheless if required one can easily introduce a gravity force variable together with the acceleration energy it generates against the constraint forces We also note that the refactored equations can handle multiple instances of each motion driver although this is not shown in the algorithm The fi rst benefi t of the proposed decomposition sweeps lies 7435 Algorithm 3 Original solver sweep 1Econf EA b 0 E A 0 E A ext 0 2 EA cstr 0 1 h F A cstr 0 T X0 Econf Ecstr N i 3sweep Solver 4 ctrl i ST iF A cstr i 5F F A ext i F A i F A b i M A i iXi 1 Xi 1 Xb i 6 qi D 1 i ff i ctrl i STiF 7 Xi iXi 1 Xi 1 Si qi Xb i Algorithm 4 Prioritization of external forces 1Econf EA b 0 E A 0 2 EA cstr 0 1 h F A cstr 0 T X0 Econf Ecstr N i 3sweep Solver 4 ctrl i ST i FA ext i F A cstr i 5F F A i F A b i M A i iXi 1 Xi 1 Xb i 6 qi D 1 i ff i ctrl i STiF 7 Xi iXi 1 Xi 1 Si qi Xb i in providing a rich query backend to and interaction with the whole robotic software architecture as we will discuss later on However to establish the link to the original ACHD solver beforehand we address the solver sweep b Solver sweep As already outlined above dynamics solvers in real world robots are very often employed to merely support motion control for trajectory following for instance by feedback linearization or to introduce active compliance into manipulators For such applications a pre composed solver formulation like in Algorithm 1 is suffi cient where all three sweeps are tightly coupled and the solver sweep directly encodes specifi c choices In the following we show how example applications can employ custom solver sweeps as a mechanism to impose task dependent policies on the dynamics solver thus unfolding the solver s full potential To start with Algorithm 3 shows the solver sweep that encodes the original ACHD solver Line 4 of Algorithm 3 shows the computation of the control torques that let the manipulator execute the desired constrained motion Yet those torques may drive the joint actuators into saturation The reasons may be plentiful for example the desired motion drivers demand too high accelerations or forces or the manipulator is close to a singularity Therefore an application may decide to saturate the torques already during the dynamics computations as shown in Eq 1 ctrl i saturate ST iF A cstr i 1 As a result the solver will not compute the real torques and accelerations required to satisfy the constraints but instead a deviated version of those quantities Afterwards it is up to the application level to decide if the resultant m