# A Design Example in MapleSim: Controlling a Robot using Feedback Linearization so today I’m gonna be talking about what a design problem actually looks like in Maple soon so we’re gonna be going through and just kind of showing you what that workflow actually looks like because I find it is really different from other tools so the design example I’ve chosen for today is the feedback linearization of a robot so it’s a control design example and so just a little bit about where maple soft comes from as a company to start so we really have a strong background in mathematics and that’s that’s where the our original maple product was developed and it’s a symbolic mathematics engine so it gives you all kinds of tools for working with equations doing analysis and it’s been a very good thing for us to build from when going into the engineering world so we started moving into engineering problems because they’re all math problems with some context essentially and so this is where maple sim came from and it’s our simulation analysis package for working with dynamic systems and we also have our applications engineering group here that does contract based engineering system solutions using maple sim and our other products and so this has really changed how we look at some of the problems that we’re dealing with and it changes the solutions that you get from your from your analyses so really when you look at engineering you’re always working with equations and and that’s the natural language of the engineering problems whenever you break open a textbook there are always equations staring you in the face so there’s lots of equations and there’s usually no code to tell you how to implement those so the odd occasional textbook might might include something like that but it has equations just describing the physics of your problem and the actual implementation is up to you so this is something that’s gonna come in with you when we look at path following example that I’m going to be presenting so we’re gonna be using these equations and building up a model here we’re gonna build up a controller based on the model that we have so the the application that I’ve chosen here is engraving so if you have a robot arm and it’s gonna be cutting a path onto a surface and it’s going to be making that mark using an engraving tool and so really you have two objectives you want to move that cutter to your desired path and you want to move it along the path at a certain speed so if you go too fast it might produce a rough edge and if you move too slow you’re wasting time so this is where it can it’s going to get a bit more into equations with control design theory but if you’re not familiar with control design Theory that’s okay because the main point today is the equations that you see here just get put directly into maple later and we can just work right in this natural formulation of the problem so first off we have a non linear state-space system and so they have this form we have some dynamic equations and you have an output equation so usually the dynamics would tell you how the joints are moving in a robot for a given input you here which would be the torques on the joints and then Y is the output which would be the position of the end effector so it’s where that engraving tool is located in 3d space so today what I’m going to look at is you can introduce new new set of task coordinates I’ll call those guys Zed and this represents a circle that I want to engrave at a specific location in space so there’s two components here and the first one the first group of two equations they form a circle in 3d space so that tells me where I want the end effector to be and then the last one is actually the arctan here that’s going to be an angle that tells you where you are on the circle and we use these tasks for de Mnet’s to form a virtual output so you can express what your virtual output is with respect to the state vector and so that this signal function becomes really important for the problem so with this Sigma with this dead function to find when these two components of your task coordinates are zero you’re on the circle that you want to be on so we want to drive those things to zero with the arc tan with the angle along a circle that angle is something that we want to have a constant velocity so that that tool will move along the circle at a constant speed and so this is what you’d see from a control design paper for the formulation of the problem and then next it would be a question of what do you do with those equations either you get the robot to do what you want it to do so there’s a little bit of notation here the lead derivative is written as LF of Sigma and that’s just the partial derivative of Sigma with respect to x times the vector F so you’re multiplying a partial derivative by vector field so if you take the virtual output and you differentiate at once you end up with an expression that looks like this so you can write out what those what the derivative of Zed is using these Leader riveted because it’s mechanical system that has a certain form and it’s a second-order system the first term here goes to zero and so that’s something that’s a result from literature and I’ve got a link to paper on the next slide but yeah if you’re not familiar with control design don’t worry about it for those of you that do know about it it says that the relative degree of this system is not 1 it’s higher than one solid state which makes sense because mechanical systems are second order systems so if you differentiate that Z vector again you can write the dynamics of the system in this form and the control input u now enters into the equation and that’s a really important part because that’s the U that we’re going to be choosing to get the robot to do what we want it to do so this is where feedback linearization comes in so we can choose our controller in this way and that’ll do something very special for us so the this is the Zed dot from the last slide and with this you what you’re doing is you’re cancelling out this matrix here in front of the U and then you’re subtracting away this term that with the LF two of Sigma and you’re adding in a new inner control loop variable so what this does is essentially subtract away the dynamics of the robot and then adds in the dynamics that we wanted to have and so you have this very simple closed loop system that says that the Z double dot is going to be equal to V and we can choose V to be whatever we want it to be so that’s how you actually get your desired dynamics implemented in the system and there’s a paper that I was talking about before so from here we’re gonna jump out and we’re gonna go into maple sim and so in maple sim what we have is a very simple looking model but we’ll drill down into the blocks so we have a controller here that I’ve designed and we’re gonna go onto the worksheet for how I design that but first I just want to show you that inside this block there’s a model for the robot that we’re gonna be analyzing so this is a crew-cut robot and what we’ve done is we have three revolute joints in here that are connecting up the different links of the robot and temporarily for this example I’ve locked down the other two joints so I’ve taken it two revolute joints and I’ve just replaced them with rigid links so that we’re only considering a three degree of freedom example today and then it’s gonna be this robot whose equations that we end up extracting from the worksheet so there’s some torque drivers here the service inputs to my system and then angle sensors that serve as the outputs to my system and so this is the system that I want to analyze the equations that I want to gather and work with so to do that and the attachments for the model we have this worksheet called feedback controller and in the feedback controller just go to the top of the sheet and that’s taking a second it’s filling the feedback controller worksheet here you have programmatic access to the model that’s sitting in your Mabel soon diagram there and so what you can do I would do this live if the computer were a bit faster because it really doesn’t take that long on a reasonable machine but you have the maple sim link model command actually gets you a programmatic handle to the model and then I can look at the subsystem that I’m interested in in this case the robot you can choose some nice name substitutions to make things more understandable or shorter so that it’s it’s easier to look at but in this case I’ve chosen them so that it corresponds to common conventions in robotics so Q are the names of your joint angles so I’ve said that the revolute joint one it’s angle is gonna be q1 4 um so the second revolution is q2 etc and then the moments about the joints are gonna be my control inputs my new one my u 2 by u 3 and then when you call this get multi body command what is doing is it’s analyzing the system it’s figuring out what a good set of coordinates are for your robot and forming the dynamic equations of the system so what we have here is a system without constraints so we have constrained free system and that’s why you see zero algebraic constraints if you have a manipulator that has a kinematic loop in it somewhere then you’re gonna have something of a slightly different form than what we’re seeing today but you can still do this kind of analysis it just becomes a little bit more complicated there’s a lot of housekeeping in this section for variable names and substitutions but what I wanted to focus on is this is the internal representation that that multi-body command has built up for your system so in the constrained free case what we have is a mass matrix XM time’s the acceleration of the joints is equal to some force vector and we can reform that into that non linear state-space form so we have this would be our X dot vector it’s gonna be formed of Q dot Q double dot our F vector is going to be the joint velocities and then the inverse of that mass matrix times VF minus u so in this case we’re lucky that VF is a finding you like it’s not lucky so much as it’s by construction when you have a robot of this form and you have torques that you’re applying directly to the joints your inputs are going to be you can separate them out as a vector and then the G vector has that inverse the mass matrix as well and one of the neat things is that you can just take this multi body mass matrix and it looks horrible and complicated but when you’re looking at it when you see these fractions that have large numerators and denominators the reason for that is that there’s an exact representation of the model here we’re not doing math in floating point coefficients that would lead to possible losses of accuracy as you’re manipulating expressions and you’re losing digits here and there as you do different operations so this is being represented exactly so that you could look at this as a floating-point number with arbitrary precision precision and then the rest of the matrix here is written out in terms of cosines and sines of joint angles so it’s something that you expect from robotics textbook but if you’ve worked much with those matrices you know that it’s it’s really hard to find examples that are simple enough that you can still write out these matrices by hand and that’s why I love tools like maple and Maple sin because these expressions are getting big they’re getting huge but I don’t ever have to work with them by hand so it’s completely fine to have a large mass matrix and if we keep scrolling down you see it just keeps going for a while computationally speaking though you have a lot of the same terms in here there’s a lot of cosines there’s a lot of sines so when you go to calculate this matrix it’s not actually that expensive so it looks complicated but the strut there’s so much structuring here that you can find smart ways to calculate it and that’s exactly what Mabel Sam is doing behind the scenes and here’s where we get into the part where it’s more directly connected to the mathematics that you get from an academic paper or an engineering textbook then a lot of other tools that I’ve worked with so we can invert that mass matrix by just using linear algebra matrix inverse and so this gets us a full symbolic inverse of that matrix and I’m not gonna print too many more expressions here because they start getting very very large and some of them the controller at the end would likely take several several pages if you ever tried to print it out but they’re completely within the scope of maple maple seemed to work with so the the bottom part of that F vector we can form by taking that inverse and multiplying it by a difference of the vectors for the forcing dr. and the inputs so you can build up your F and G vectors from that nonlinear system representation your forward kinematics are also something that’s very easy to get so this is that relationship that gives you the output equals H of X that y equals H of X and from that multi body object you can use the gap free motion command so what we’re doing is we’re just asking for the relative translation of this frame relative to ground and so this is you can see from the naming convention the mean is the top level of the model so we’re looking at the robots fifth link and f2 is the frame the second frame which is where the end effectors tool would be we can express those forward kinematics with respect to state variable and so you get these equations that are again giving you what your input equations are the virtual output we can define so I’ve got an expression here that’s the circle so really in three dimensions this this is a cylinder intersected with a plane so that gives you a circle and then we have that art and that angle on the circle for our position portman so we can define those naturally in Maple and then what you can do is you can say I want to form that Sigma function by substituting the forward kinematics into my task ordinance so you’re doing this evaluation and again you end up with something that’s complicated but it’s an exact representation of the models behavior and an exact representation of what your output should be so there’s no approximations here and you get something that’s easy to work with in Maple and easy to analyze so for the next step we’re going to form the inner loop controller so if you remember the outer loop the feedback linearization controller gives us this relationship this double dot is equal to V and here I find it easier to start from the bottom and work up so for V we have that we’re gonna have a proportional gain times the Zed vector minus a derivative gain times the z dot vector and here we’re doing a difference because we have a desired velocity in the third coordinate of the task coordinates so this is saying that we want the derivative of that arctan to be equal to one and that would give us just a of one Radian per second around that circle so we can define what the proportional derivative gains would be and you could make these parameters for the system but I was just putting together a simple example here and so I’ve put them in as constants but you can make them as parameters for the blocks that you see in the maple sim diagram to formulate the outer loop controller if you remember we had the expression from the slides that looked like that and we can form these leader riveters by actually just differentiating what the virtual o´clock was so those coordinates and task space as expressed with respect to the state variable and so what I’m doing is I’m taking a leader derivative with respect to those X variables I multiplied by F etc etc so you can form your L F Sigma your LG Sigma and if you remember I said on the slides that goes to zero and you can verify here that it does go to zero and you’ll see that it’s exactly zero so it’s not that if you’re doing some computations numerically maybe you’d get a matrix here that has small entries and you’re left with the question of does this really mean it’s zero or does it mean that there is some numeric errors I’m worried that I didn’t pick up or there’s some uncertainty when when dealing with it the numerical computation sides about what small values actually mean but here are the symbolic manipulations we see that it’s exactly zero and so you know that this really is a system that has a relative degree greater than one okay so you can form up the other leader riveted and there’s a little trick you have to use to minimize maples computation work when inverting a matrix with large symbolic entries but that’s documented in the help page and so you can form the inverse of that LG fine so if you look up here and compare the two it’s it’s a very natural way to implement a controller there’s some name substitutions I have to do but that’s really more accounting than anything else here so once you have this expression we have these giant equations here that describe what our inputs are we can go and create a medela component from them so I just want to point out a few things here so we have this maple Touma delica command that can take the equations for the controller and it’s going to generate a component and so that component was that block that I pointed out on the maple sim model at the top level and so it’s going to be a type named feedback controller and we can define that the inputs are the joint variables and the outputs are the control inputs and when you use the set component command this is going to take that Medela component and attach it back to your maple symbol so if we go back into the meat Wilson model you’ll see that under the custom components listed here there’s now a feedback controller component so this is that medela qey that I’ve just attached to our model if we go to back to the top level of our system here you can see that this component that I’ve added is that feedback controller so every time you regenerate that component this this little custom component here will be refreshed so if you run the simulation which I’ve done already you get this 3d visualization that you can actually see an animation of your results so in our case we’re going to see that robot track onto that circle and start doing its engraving tasks because it goes around and around so you can see that the robot drops down quickly towards a circular type path and then can more slowly approaches that circle until it finally gets there and now it’s moving around at a constant speed which is exactly what we wanted one of the benefits of the feedback linearization controller that we used here is that unlike the PID controllers there are terms in here that account for the centripetal acceleration needed to stay on that circle the PID controller is like I was saying before they’re a linear controller and so they don’t handle these nonlinear constraints perfectly so what’s happening if you use a PID controller instead is that you’re actually going to drift outside of this circle and you know you’re not going to track the circle perfectly you can add additional terms to PID type controllers to try and compensate for that but then you’re already into a nonlinear control design and you may as well use more sophisticated technique so with the results that we’ve generated here you’ve got a nice animation that shows you exactly what your what your model is doing so it provides very intuitive results and it’s taken in CAD geometry here to show what the actual robot will look like and if we head back into the maple sim model one of the things that I really like about this is that the individual links in here actually come from CAD diagrams so if you look here there’s STL files that have been attached to this model it’s pulling in that 3d geometry from those CAD files and it’s also pulling information about their inertia center of mass and mass you just have to insert what the mass density is so whether the robots made of steel or aluminum or what have you and if you’ve ever tried working with robots then and coming up with their dynamic equations it can be very difficult even for a simple robot to determine its inertia and center of mass by hand so being able to automatically import it from other tools is a great advantage and it really helps your simulations come to life so that’s all we have for our presentation here today and I’d like to thank you for joining me here I just like to remind you that we have plenty of other resources on our website there are tons of models that we’ve made available through the maple sim model gallery to help people see how you can approach different design problems so these are models with analysis worksheets attached from all kinds of different disciplines and engineering so there’s something there for everyone we have other recorded webinars on our website we have our online help pages that show you what the commands are that we offer and what they do and so there’s lots of resources and I encourage you to explore our website it’s a great way to get lost for an afternoon