CST torque control mode under EtherCat

[WillyTuring]

Hi You all, Hi Arthur, It's been quite a while since i updated the project progress due to a lot of external factors, and this question might not related to the Soem directly, but still hope i can get people get into discussion since Torque control is such a common practice in reality.

My questions are the following: 1.Determining P gain and I integral time. In theory the integral time should be in inverse proportion to the constant time of the motor, but what i've observed is that in practice, it's nothing but just a coefficient in front of the integral of the error, so the way i used to determine those two parameters was just trial and error to find the fastest while still stable response, Not sure if my intuition is is correct about the integral time chosen.

2.Compute viscous coefficient Since torque τ=I·a-bω+mgrcos(θ), i'm not yet figure out how to solve the viscous coefficient b in this case. What i did was i ran the motor with no load and the shaft in vertical plane trying to get rid of gravity term, so that i can solve viscous coefficient b with equation of τ=I·a-bω. I then did multiple rounds of bump tests with different torque target setpoint, soon as the motor reached the torque setpoint, i record the velocity then after 1s(250 cycles*4ms), i record the velocity, then calculate the angular accceleration in radians/s^2, then basically compute b. But what i found is as the torque setpoint increases, the viscous coefficient drops rather than increase, can anyone see what i'm missing here?

3.The third question is about General principle of using CST mode to control position. ①My thought process is i use e.q of τ=I·a-bω+mgrcos(θ) to get the torque i need in terms of acceleration. ②Since i can control the time and knowing the speed limit by V=i(t)R+L·(di(t)/dt)+kω; i can influence the speed. ③On top of that i can then use the if condition every cycle to check if the motor reached the target position or not to finely dictate the stopping point since i don't think the position in CST mode is via calculation it might not be that precise.

Anyhow, hope anyone who have practical experiences or knowledges on using CST mode can chime in to form a discussion here.

[WillyTuring]

4.Here actually comes another problem by using e.q of V=i(t)R+L·(di(t)/dt)+kω, i ran the experiments and when the motor reaches the stable point which inductive term goes to 0, i then back calculated voltage V using the speed under stable situation, seems pretty legit, but the problem is i actually want to use V to find the stabled speed not the other way around, but i'm not sure how can i get this V value.

[ArthurKetels]

Some answers:

1. You are referring to a conventional PI feedback controller. There is lots and lots to find on the internet about this control strategy. It is useless to talk about this if you do not know what you what to control, and the properties of your plant.
2. You torque model is one of many, and perhaps not a particularly accurate one for the plant you want to describe. In "control speech" a plant is everything in-between your controller and the state sensor (position, velocity, force, temperature, pressure etc.). The better your model ,the better your prediction, the better your control. What you did with the bump test is called system identification. Very good! But as you have noticed the observed behaviour did not fit your model. Time to adapt your model!
3. What you say is not wrong but rather useless for control. You need to calculate a trajectory from A to B where position, velocity, acceleration are known at each instant in time. Then use those references in your model to calculate a torque setpoint. If you model would be perfect then you will reach point B at the exact time. However any disturbance or error in the model will make your plant deviate from the reference trajectory. The feedback loop is to correct this error in the most optimal way (if that is good enough is a whole other question).
4. From you torque setpoint you can calculate the current in steady state. Calculating V for any omega is then very straightforward. On the other hand it will not get you where you want to go.

Recommended strategy to use torque control for positioning.

1. Define a model for motor, gearbox and load that is accurate to at least 90%.
2. Do system identification to determine the parameters of your model (or get them from the datasheets when available).
3. Calculate a trajectory from A to B that is feasible (within constraints of your system). Extract position, velocity and acceleration for each time step.
4. Use you model to obtain a setpoint for each time step.
5. Send setpoint + feedback signal from 7 to your motor drive.
6. Compare actual position, velocity and acceleration with those from 3.
7. Feed the error signal in a feedback controller (often proportional controller is enough).

Your motor model is adequate. For the other parts I often use an inertia and friction model.

Torque = inertia acceleration + gravity compensation + viscous friction coefficient velocity + coulomb friction coefficient * direction.

The problem is that the viscous friction is a function of temperature and torque. So you would need a look-up table to find the best value. Do not overdo this as your model need not to be perfect. The same is true for the coulomb friction.

When the inertia is not known then you need an observer that calculates the inertia based on the observed acceleration in relation to the torque. As this is a circular dependency a lot of care needs to be taken to keep this stable. The simplest solution is to employ a low pass filter to this observer.

[WillyTuring]

Hi Arthur, Really appreciating your help as always. I think you are right on the modeling part. The theoretical model always needs to be adapted with the practical situation.

But theoretically i still have one concern, after pondering the suggestions you gave above, here's what i concluded: Modeling the real world motor control system comes down mostly to the torque-acceleration equation modeling in my case, if this can be approximate to say 90% of the reality, then the only concern i have is what about the "time"? Sure we have fixed communication cycle, we pretty much know when does the slave generate sync0 interrupt, but how long do they actually dictate the motor to move before next cycle starts? For example if we are in 4ms cycle, does the motor move 2ms or 1.5ms?How do we know that? Since this is an input needed to calculate both the rotational speed and position.

[ArthurKetels]

A good slave controller will have this information in the datasheet (hint, very few have). But it is easy to identify by doing a sinusoidal torque setpoint and then capture the motor response. Calculate the phase delay to get your control latency. This measures the whole "plant". We do not care if the delay is caused by the master, the slave or the motor (inductance). As long it is constant we can compensate. Of course we try to minimise the total delay because that will give the largest phase margin, and improve total system response.

[WillyTuring]

Hi Arthur, So what you are saying is that i plot the input and the response sinusoidal curve on a time scale then calculate the phase delay(time delay) between those two waves, in this case the execution time will approximately be "cycle time-delay time", right?

[ArthurKetels]

Yes on the first, no on the second.

Assume your controller sends torque setpoints at time A1, A2, A3 etc. , and the interval between each is constant. If then the actual change in torque happens at B1, B2, B3, the interval between two B's and two A's is exactly the same, only offset in time. So the integrated torque setpoint over an interval does the same amount of work. And it is this work that actually moves mass.

If you only would use a feed forward controller then we do not even care about the time delay between A1 and B1. The exact path is still followed. However this delay is important when we want to employ feedback. The encoder and other measured values of your drive are sampled at C1, C2, C3 and they become available for your controller at D1, D2, D3.

You can only apply feedback at A2 when D1 arrives before A2 (plus some time to calculate the response). Otherwise you have to apply the feedback at A3 and you loose a whole cycle for feedback correction. Some drives and communication systems are even worse. For simplicity we assume D1 arrives before A2.

The amount of work that has been done by A1 is the integral over time C1-B1, and this is smaller than the interval A2-A1. To calculate the error value at sample time C1 we can look-up the trajectory reference at time A1+(C1 - B1), and compare it with the value we measured at C1. This error signal is then valid for time C1. We can use it as described above (step 7) and add the correction at A2.

The nice thing about this method is that it also works for coordinated moves with multiple actuators running at different intervals. One servo drive can run at 2ms while an other one runs at 3ms cycle. They are still perfectly delay compensated and deliver synchronous motion, only the communication is not synchronous. The same is true for two drives that have different (B-A) or (C-A). As long as you can identify the time offsets and do proper bookkeeping it works.

A lot of people programming control loops are just too lazy and forget to properly account for time effects. Time is just as important of a parameter as torque, velocity and position are.

[WillyTuring]

Hi Arthur, Very clear reasoning. Following along with the time stamp you gave above, let's see if i understand your point correctly.

So basically our goal is to get the "error" so that we can add this error to the next command cycle. And the real work only be done at time duration of (C1-B1), and we can get this let's say the position increment which is the actual movement sent back from slave; whereas our command movment is calculated based on the time duration (A1+(C1-B1)), then the difference of those two is the error we are seeking after, am i understand your point right?

The problem is here is the timeline: A1, B1, C1, D1(command sent, command received, sampling, feedback arrived at master). We know (D1-A1) which is the communication cycle. And i think we can roughly calculate (C1-B1) given speed and position increment from the actual feedback increment. If (C1-B1) is consistent, then we can just use this time duration as the "t" needed for the command calculation, and then compare it with the real sampled feedback to get the "error" term then feed it into the next cycle, i think in this case the error will be smaller, so why do we need (A1+(C1-B1))?

[ArthurKetels]

You are confused with cycle time and offset. The cycle times of A, B, C and D are all equal, they are only offset in time. We calculate the trajectory and torque setpoints in time domain A. We know they are executed in time domain B (witch is only offset from A with a fixed time). The torque setpoint is effective for the whole cycle time. Ideally we want C at the very end of cycle B, because then we see the result of our action. This is not possible because we then have no time to communicate the result back to our controller before the next cycle. The measurement at C2 is the combination of the action B1 and B2. Combined, C2 covers a whole cycle. The formula (A1+(C1-B1)) is just to calculate the offset with respect of our reference trajectory. And that is what the error signal is about, to compare the reference trajectory with the real trajectory. This is only accurate when the time instant of reference and actual state is the same.

[WillyTuring]

Hi Arthur, I don't think i confused the "cycle period" with "the action inside each cycle".

According to your expression: "Assume your controller sends torque setpoints at time A1, A2, A3 etc. , and the interval between each is constant. If then the actual change in torque happens at B1, B2, B3, the interval between two B's and two A's is exactly the same, only offset in time."

I'll lay up some terms and opinions, correct me if and where i'm wrong: 1.In each EtherCat communication cycle, "ec_receive_processdata" is always in the beginning followed by command calculation then followed by "ec_send_processdata". Which means in each cycle we can always read the execution result of the motor from last cycle that is the "Real trajectory".

2.So let's label 1, 2, 3 as three consecutive cycle periods, whereas A, B, C, D are four actions inside each cycle representing ①D:Feedback arrived; ②A: Command sent; ③B: Command execution started; ④C:Encoder sampling; So D1, A1, B1, C1 are four actions inside cycle period 1 and always started with "reading feedback" first.

3.In your convention, we use "Reference trajectory" as for our desired trajectory, "Real trajectory" for the actual feedback trajectory we get from motor encoder.

4.In cycle period 1, motor execution time duration is (C1-B1); In cycle period 2, master read execution result from period 1 which is the "Real trajectory" at time D2; So in A2, we need to compute the error and then send to the slave, and Arthur, you are suggesting we get the value from our calculated reference trajectory at time (A1+(C1-B1)) as our "Reference trajectory", then compare it with "Real trajectory" to get error and send to the slave?

In getting "Real trajectory", we don't need to care about the absolute value of time duration in (C1-B1), but we do need absolute time duration value in getting the "Reference trajectory" since this is the trajectory we calculated against time.

So my question is: How do we get this absolute time duration value? I think this is the key, because we have the absolute starting time point A1, so if we know the duration, we can just continually add on to get the absolute time value in the time line then retrieve the reference trajectory value.

[ArthurKetels]

Yes, I agree on your summary. But as you are now drilling down in to the details I have to adjust one mechanism. Above I mentioned that although we would love to get Cn-1 at cycle n this is often not the case. It also depends on how we organize the EtherCAT PDO exchange.

In SOEM the example begins the PDO cycle with reading the measured values from cycle n-1, then does calculations and ends with writing the new setpoints for cycle n. This is advantageous when the time spend in the PDO function is short with respect to the total cycle time.

But what happens in the slave with the EtherCAT packet?

In a free-running slave (no SYNC0) the measured state is updated as often as possible. Each time a new encoder value is available this is copied in the PDO data. Often overwriting the previous data set as the master did not retrieve it. This happens when the slave internal update rate is faster than the PDO master cycle. Ideally the slave would also time-stamp the encoder value, then the master would know exactly when the sample was taken. If there is no time-stamp and the internal slave update rate is not known there there will be jitter in the sample as the slave and master rates interfere. The effect is that Dn is actually a sample around An-1.

A synchronized slave (SYNC0 active) takes a measurement sample C just before packet A arrives and executes the setpoints in A at B (where B-A is minimal). The return of the PDO to the master results in the same situation as with the free running slave, Dn is a sample around An-1. There is no advantage of using a synchronized slave over a free running slave other than that there is minimal jitter on C and B. And depending on the use case this can be a big advantage.

With SOEM we have the opportunity to change the PDO transfers to our own liking. We can have two PDO transfers per control cycle. At the start we do a full PDO transfer to get the freshest possible servo state, but only with input data (LRD). So ec_send_processdata and immediately ec_receive_processdata. So we get Cn-Dn. We the calculate the torque setpoint and do another PDO transfer. This is outputs only (LWR), An-Bn. This is of course only possible when we have enough bandwidth. Very effective when having to use slow cycle times (>=5ms).

At fast cycle times (<=1ms) the inertia of your system is mostly dominating and not much happens between cycles. Then the latency of B and C do not change the dynamics to a point it is noticeable. There are however a few applications where timing points must be known to below 1us in order to get the required precision.

Now to your last question. Yes it is important to generate a trajectory where time is exactly known given start and end conditions (position, velocity and acceleration). Luckily there are open source solutions available. Examples : https://ruckig.com/
http://linuxcnc.org/

[WillyTuring]

Hi Arthur, You bringing another really good point here that is the inertia part: "At fast cycle times (<=1ms) the inertia of your system is mostly dominating and not much happens between cycles".

I just did remodling with the motor in which the model uses (ΔTorque) as the input and (Δpulse position) as the output and i made the time constant in the unit of (Communication cycle, in my case 4ms). And i'm concerning with the inertia since the torque is constantly adjusting in each cycle.

Anyway I'm going to test it to see what happens then talk to you back.

[WillyTuring]

Hi Arthur,

I have tested the model back and forth then stucked.

I have found the relationship between the velocity and the position(unit: pulse) meaning if if i have the motor velocity in the next cycle, i can predict the position increment then the absolute position pretty accurately. I have also found how does the motor follow the torque command meaning the relation between actual torque and the target torque(commanded torque), so i know what is the actual torque in next cycle given the torque command in this cycle.

So seems like the only puzzle left is the relation between the actual torque and the actual velocity. If i can build the model between those two, then i am able to give out the torque command in this cycle, compute the actual torque in the next cycle, calculate the actual velocity in the next cycle and predict the actual position in the next cycle.

But i just wasn't able to build the model between Actual torque and Actual velocity either in pure mathematics(through data crunching) or through mechanics model.

I attached the actual raw data captured in excel, and would love to hear your thoughts on this. dataCaptured.xlsx

[ArthurKetels]

Thanks for the data. This explains a lot. Unfortunately you made it too complicated for yourself. It is much easier if you write data in SI units. I have no idea about scaling of your variables. Also keep it simple, change torque setpoints from zero to +max and back and later from zero to -max. For phase measurement you should have stuck with the proposed sinusoidal setpoints. Then you could do an fft and extracted a precise phase shift and thus latency with sub cycle accuracy.

Anyway attached is a plot (made with kst2) from your data and annotation. From the data a good model seems to be inertia plus static friction plus viscous friction. For you to figure out is how large the constants are. With proper tests you can identify each and every one of them. You then have the relationship between acceleration and torque. And that is all you need to control your motor with feed-forward. A bit of feedback will then give you the required performance.

[WillyTuring]

Hi Arthur, I have actually done the plotting like yours as well except i did in Excel which i'm more familiar with and i actually did toggle the command from 0 to max and 0 to -max back and forth.

The reason i did it with different setpoint number is that i found out the constant and coefficient of the model changes quite a bit when different setpoints are used, that's why i used 10, 20, 30, 40 etc(btw. The unit of those number is 1/1000*rated torque, so 150 means 15% of the rated torque, no reducer involved at this moment).

[WillyTuring]

Hi Arthur,

I have been had some progress testing and tweaking the model, but got stuck when i toggle between different target torque setpoint. Here's the situation:

The data in the image above is the data i captured and processed in cases where torque is decreasing, like switching from setpoint 15% rated torque to 2% rated torque. The x axis is velocity, the y axis is acceleration and both of them are in SI unit. And please note all 6 graphs are tested seperately, for example, for the first(up-left corner) graph, i just toggle between 15% rated torque and 2% rated torque, then cut out "decreasing part" data then process it(cause this is the section where i have some problems), so if it's 3%, meaning i'm toggling between 15% and 3% of the rated torque. This is all good and consistent between different cases and fits the model acceptably.

Here comes the problem In the image above is where i mix different torque setpoint together, as you can see the format in the left column where i encircled with red, basically i go up to 15% rated torque then come down to 2%, then up to 15% again, come down to 3% etc. The problem is in this case there seems no clear relation between velocity and acceleration in the "descent part". Whereas the "ascent part"(up to 15% torque setpoint) is still fits the model greatly after some data process.

What's your thoughts on this situation?

[WillyTuring]

Hi Arthur,

Quick note before i give out some quick updates, i have some thoughts on explaining what's the issue i mentioned in the last comment, but still would be really helpful to know what's your thoughts on it to gain a stronger understanding about the system.

Quick update: I think i have now found an acceptable model at this moment for the system awaiting for indepth testing. At this point with the model, i should be able to command torque in Current Cycle, combined with the current velocity, predict the motor position in the Next Cycle.

But since i'm now testing the motor standalone without reducer, the speed would be much faster, and i'm actually wondering do you have any suggestions on how should i construct a Trajectory for the indepth test in this case.

[ArthurKetels]

Doing tests with only a motor and no inertia or other load is bound to give some strange results. The reason is that small unmodelled variations can have a big effect in the measurements. When you set a few percent torque you are close to the friction of bearings and seals alone. In a theoretic perfect motor with no friction a positive torque of a few percent should still increase velocity. As you can see it is not. There can also be control effects at play that are not fully understood.

Two possible mechanisms : a) At certain velocity the motor or bearing comes in a resonance, this causes energy loss and therefore erratic deceleration. b) The ripple torque of the motor at higher speeds can interfere with the torque controller in the drive causing erratic torque values.

And there are plenty more. Important is that when the load of the motor increases most of these effects disappear. System identification only makes sense in operation conditions with real loads. Many servo manufacturers give a range of load inertia vs. motor inertia where they guarantee performance. And often too low a load is as bad as too high of a load.

But no matter what, you are learning a lot about control systems, models and identification! This time is not wasted.

[WillyTuring]

Hi Arthur,

Thanks for the reminder:

Many servo manufacturers give a range of load inertia vs. motor inertia where they guarantee performance

I'm super grateful and feelling fortunate to have those helpful practical suggestions from an expert.

The time is not at all wasted, now i'm thinking about attaching some external heavy rods to the motor shaft then continue the test, in this case i don't need to get a reducer immediately if i can have some external load to stretch the motor enough, right? What do you say about this?

And Arthur, do you have any suggestion in this case without the real environment like the robotic arm to configure a trajectory solely with the external rod to test if my model is workable, my idea is that if i am able to control the trajectory with CST mode in a rod, i could do it with the whole arm, the inverse kinematics part is not a challenge to me, i have done it with CSP mode previously with the whole robotic arm.

[WillyTuring]

Hi Arthur, I have been tested the model under CST mode, i have had some success, but another big challenge emerged.

We have always talking about Feedback loop, the problem i'm facing is that after attaching a rod to the motor shaft, i need to account for the gravitational torque from the external load then update the torque command, But since the actual torque can not change instantly, rather the actual torque follows the target torque in a second-order system manner(PI controller).

Which means updating the target torque in every cycle is literally meaningless due to lack of reaction time; so my question is given the situation what's the best strategy doing feedback control?

[ArthurKetels]

The read out of the actual torque value is low pass filtered. The actual torque as evidenced by the acceleration in your plots is a predominately first order response (the effect of I in the P-I controller is small). Also the time constant of the current controller is very low.

Compensating gravity is something you do in your model and not in the feedback loop. You know the inertia of the rod and the position relative to the shaft, so you can calculate the correcting torque. Gravity is a known quantity and not a disturbance. Feedback is intended for disturbance and unmodelled effects.

Updating each cycle still makes sense. Even if there is lag due to communication and other delay's. As long there is enough phase margin between plant and controller the system still goes to a controlled state. What is true (and your worry) that you can not control at maximum frequency (Nyquist theorem).

Suppose you are updating over EtherCAT at f=1kHz, the maximum frequency you can output is f/2. But your total response is f/16. The maximum phase delay is 90 degrees (this is when command is fully decoupled from response), this is then f/4. This means there must not be any frequency components faster than f/4 in your feedback signal. The easiest way of doing that is have a rate limiter on your feedback signal. So you limit the change of torque setpoint to x Nm/s. Adding a low pass filter would worsen the phase margin.

As you see, a rate limiter on torque feedback is actually a form of jerk control. And this closes the circle, jerk control is a necessity yo be able to control force/torque. You apply it at the trajectory generator so you will have a stable model prediction, and you apply it at your feedback controller.

[WillyTuring]

Hi Arthur, Such a grateful feeling to have you got back to me, concerning your points:

The actual torque as evidenced by the acceleration in your plots is a predominately first order response (the effect of I in the P-I controller is small).

Yes, in essence it's a second-order system, but due to the small Integral term, it is a predominantly a first-order system.

Compensating gravity is something you do in your model and not in the feedback loop. You know the inertia of the rod and the position relative to the shaft, so you can calculate the correcting torque.

There's a problem with gravity compensation, back to the backbone of our dynamic equation "Applied Torque + Gravitational torque = Inertia Acceleration + b velocity + Friction". The gravity term appears on the left side of the equation, as the motor rotates, the gravitational torque changes, so in order to have our target acceleration, the Applied torque needs to be changed accordingly, the problem is the Applied torque is the actual torque not the target torque, meaning even if we change our target torque constantly while the rotor is turning, the motor still needs time to respond, the consequence is that the Applied Torque will mismatch with Gravitational torque( due to motor rotation) thus cause the acceleration to change unwantedly and ensue with undesired velocity and position. Currently, i am trying to model how the actual torque will change given target torque command, and then trying to predict the acceleration, velocity and position and in turn guiding the gravitational torque prediction. But the innate problem with all models is there is only a feedforward process but no feedback correction, this is the feedback correction step i was asking.

Suppose you are updating over EtherCAT at f=1kHz, the maximum frequency you can output is f/2. But your total response is f/16. The maximum phase delay is 90 degrees (this is when command is fully decoupled from response), this is then f/4. This means there must not be any frequency components faster than f/4 in your feedback signal.

I understand the logic behind the updating and feeding back frequency, but may i ask you Arthur, how do you get the response frequency f/16?

[ArthurKetels]

I think with your mechanical time constant you have enough phase margin that the delay is not really noticeable. But if your want the optimum solution; use a lead-lag filter for feedback. You can google it how it works.

The f/16 was just an example. When you control a robot arm the mechanical time constant is not more than a few 10's of Hertz, often even lower. A 1kHz control loop and a few kHz of current control bandwidth are more than capable to have fine control.

[WillyTuring]

Hi Arthur, Please forgive me lacking of practical experience on this matter, i have a few quick questions: 1.How does Phase margin and stability manifests itself in our case?

2.Analysis in frequency domain is super clear and straightforward, but at the end of the day we still need to send command in the time domain, i just still wasn't able to appreciate fully about the utility of the analysis in frequency domain to be honest, e.g. the step response of the first-order system, sure it can give you a better understanding of the matter in frequency domain, but when it comes to reality, the time domain result and exponential equation is actually more informative in my opinion.

3.About the mechanical time constant of the robotic arm, i'm not sure if it has that slow time constant from my experience, even when i operated it under CSP mode, for the full range movement it usually takes no more than a few seconds in a high speed, yet current control loop is only 250Hz, i guess it comes down to the control precision, isn't it? sure when we have this gap between the two time constant, we sure be able to tolerate much more position error relative to our model's prediction, is this the point you are making?

[ArthurKetels]

Phase margin: Assume a set-up with a servo motor with its shaft oriented vertically and attached a leaf spring sticking out horizontally and at its end a mass. Gravity has no effect and the response is the same for every orientation of the servo motor. This is a classical mass-spring system. The goal of the servo motor is to keep the mass at the same place.

In a static case no torque needs to be applied, the system is stable. We then introduce a disturbance at the mass. Because of the spring between the motor and mass an oscillation occurs. For feedback we have a sensor that measures the position of the mass in relation of the position of the motor shaft. Our controller has the sensor as its input and torque of the motor as output setpoint.

When there is no delay between the input and output we have a phase of zero degrees. A simple PD controller will dampen the oscillation very quickly. When the oscillation frequency is f then the period p=1/f. When the delay is 0.5p we have a phase delay of 180 degrees. Our controller will be very unstable as it amplifies the oscillation. Each control effort is working in exactly the wrong direction. When the delay is 0.25p the phase delay is 90 degrees. No matter what we output as control signal it has zero effect. It does not worsen the oscillation but it also does not improve. So anything between 0 and 90 degrees will generate feedback that is effective. To be really effective the limit is often put at 45 degrees. This is where the effective and ineffective feedback are equal. This is then what you call the phase margin. The system designer defines a specific phase margin depending on the precision requirements of the system. When you want very accurate compensation of disturbances you have low phase margins.

All real world mechanical systems behave somewhat as a mass-spring system. And all control devices have feedback delay. So there is always a frequency at witch the phase margin is spend. This is the critical frequency. The control system can not output any feedback frequency higher than the critical frequency. It is a noble goal to reduce feedback delay, because it will increase the critical frequency. But there is a practical limit what you can achieve. High frequency feedback leads to audible noise. It will also increase the torque demand of the actuator because there is very little time to correct disturbances. Imagine a servo system with zero delay, an infinitely small position disturbance will generate an infinitely high torque for an infinitely short time. You understand this is not how physics works. Any good control system has jerk limits. You can not change torque infinitely fast.

In above I did simplify things a lot. Many real world problems are not that simple, and often they behave in a non linear way. But the principle is still valid. I hope this also makes clear why predictive feed forward is so effective, feed forward has no phase delay.

The natural frequency of a robot arm is not very high. Just try to tap on the end of the arm when the robot is at stand still. You will visually see the tip vibrate. If you instrument the arm with accelerometers you can figure out the precise response of the whole arm and its parts. As a rule of thumb, when your control feedback is 10x faster (at critical phase margin) than the natural frequency you have fine control.

[WillyTuring]

Hi Arthur, I agree with pretty much all of the theories you laid out above. The theory you brought up here is a really good one for the discussion of "Theory and Practice", here's what i mean:

1.First of all, bode plot is by definition a plot of the steady-state response of system output to a sine wave input. In Fourier series thoery, sure all the input can be decomposed to numerous sine wave in superposition. But at the end of the day, our input is mainly step function.

2.Second of all, Spring-mass system in essence is a second-order system, and i think this is actually the fundamental reason that we have term "Natural frequency" defined and derived from mathematics in the first place. Combining point 1 and 2, and like we discussed before, yes the motor dynamic model is in essence a second-order system, seems like if we give it a sine wave torque command relative to time, we can replicate the theory in practice and then discuss about the frequency of the input etc, but my question is since there's absolutely no reason for us to give out sine wave torque command, what's the practical point of us talking about the frequency in this case?

3.Yet i totally agree with you on the phase shift when it comes to feedback control, even if with negative feedback, it can still cause unstability if phase shift went wrong. But despite this, i still don't think the term "frequency" has too much practical guidance in this mechanical system in our case, even if when we are talking about the basic SVPWM in the servo, when we command different torque setpoint, we are only changing the magnitude of the vector, not the rotational speed directly, the rotor's velocity is determined passively due to back EMF.

So, to summarize my thoughts, i think you made a really good point on the significance of feedforward control and the consideration of control precision, and on the one hand this is exactly what i'm trying to do at the moment—improving the model. But no matter how good the model is, feedback correction is a MUST, notice here not only i'm talking about the feedback due to the disturbance but also the compensation/correction for the model itself, otherwise there's no way we can avoid the drift in the long run.

So, if we dissect our problem a little bit: ①I have a pretty acceptable model in which the input is torque, the output is the "Predicted position"; ②I set an updating cycle at 20 EtherCat cycles for the first two updating cycle and 10 EtherCat cycles for the rest, the way i determined this is mainly due to the fact the time constant of the actual torque following the target torque is 4 EtherCat cycles, so with 20 EtherCat cycles, it always guaranteed to be reached at the target torque, so that it's easier for me to make my prediction and feedforward. ③OK now at the end of each "updating cycle", we surely have difference between our "predicted position" and "the actual position", if we don't do any correction for this discrepancy continuing with our feedforward torque command for next cycles, there's literally no insurance that our position won't drift or at least out of our prediction. One might argue if the model is good enough, it might be moved more in this updating cycle and less in the other, and ends up evenly, well, if i run the motor long enough(at least several seconds), sure no problem, but i don't think that's the best strategy.

So how should we conduct this "Feedback correction" appropriately? I have some thoughts and implementing them at the moment while discussing with you, will share it with you later on if i have any luck.

Final thought, THANK YOU Arthur for coming along! Since this is a personal project now, sometimes it does feel a little bit lonely doing it alone without any in-person discussion with others, i'm SUPER grateful for your help and accompany. Cheers!

[ArthurKetels]

Talking about differences between the time and frequency domain is useless, they are the same. With simple transformation you can change from one domain to the other and back again. If you feel more at home in one domain, please use that. The end result will be the same.

As I stated in one of my first post in this thread it is a good idea to have feed-forward and feed-back. It does not really matter if disturbance is external or a misfit in your model. I call disturbance anything that causes the model to deviate in the real world.

For feed back it is best to have a layered approach. In the hierarchy of jerk-force-velocity-position, an error in the higher node feeds back in the node one below. Error in velocity generates force feed-back, error in position generates velocity feed-back. And yes, if you can measure it, and your servo drive can handle it, error in force generates jerk feed-back.

Now you might ask, how to generate a motor torque setpoint from the velocity feed-back signal? For this you use your model. The model is the best available translation from position to velocity to torque. All non-linearities are kept there.

The advantage of the layered approach is that each loop can have its own response time and specific clipping and filtering. Force control has much higher bandwidth than position control.

[WillyTuring]

Hi Arthur, Progress Update!

I finally having some luck controling the velocity and position increment via torque command(CST mode), here's the graph from test(raw data attached in the file): Basically, what i'm doing here is i was trying to do is to have the motor rotate 1 rep(2Π radian), the trajectory should be the motor first accelerate to the velocity at 2Π rad/s, then rotate constantly at that speed, then decelerate to 0 rad/s and stop(Trapezoidal curve in terms of velocity).

Then a new problem arose at the deceleration part. Basically the problem is controlling the motor to stop at exact the desired position, i'm aware "exact" is not an accurate word in this circumstance, but i think you get my point, rephrase the problem is that how to control the motor to reach the desired position at velocity 0, because if the motor is not zero or near at the desired position, it's hard to stop with solely the gravity compensation.

I would really like to hear what's your say on this one, thanks in advance. dataCaptured.xlsx

[ArthurKetels]

I have been rather busy lately, hence the delay.

1. The aggregation of cycles is NOT optimal. You should calculate the proper feed forward and feed back signals each cycle. The fact that there is latency does not change this. For feed forward this is not needed and for feed back you only have to compensate the reference values. If your measured values have 4 cycles delay then compare them with the reference setpoint of 4 cycles ago to calculate the correction value.
2. When you have your model reasonably accurate the deviation from your required trajectory in position, speed and acceleration should be minimal. The feed back loop should take care of the rest. Feed back loops should be implemented as cascaded loops. On the lowest level to correct acceleration, second level for velocity and top level for position. You could do with only the top level position feedback, but you will notice the disturbance rejection and overshoot of a cascaded loop is much better.

[WillyTuring]

Hi Arthur, No worry, totally understood, it’s grateful enough for me to hear back from you. I actually did exactly what you just described above at the very first beginning, and the result not turned out well, i totally agree with you on the delay part that it will not be a big deal if delay is the only thing there. Yet the main problem I’ve noticed is the “unpredictable response”, hear me out: Let’s approximate the current response with first-order system, let’s assume time constant is 5 EtherCat cycle, when we update our command every EtherCat cycle, it has several consequences: 1.Since we only give it limited time to response, the response is more complicated and unpredictable as we know it becomes more predictable and meaningful from physics aspect as it approaching the steady-state. 2.Since there’s only 1/5 time constant duration for motor to respond, the increment is fairly low so that the target command needs to be really large to accomplish the same result, the bad thing about this is that whenever there’s a turning point in position, the change in target command might out of the roof. Let’s have a simple example here say i give it a torque command 1nm, since there’s only 1/5 time constant duration for it to move, let’s say the feedback gave you a 0.1nm, even if you can predict that happening, your model predicted to be 0.09nm, you still need to send out your command at 1nm, there’s a mismatch between those two and then you do the position error calculation from 0.09nm and 0.1nm for the feedback, there’s work required to predict the the actual torque 1/5 time constant duration later, apart from the difficulty of doing it, and the constant change easily yields unstable response, and less meaningful from my point of view. That’s why i choose to update command every 10 EtherCat cycle, it yields much more predictable and stable result.

[ArthurKetels]

1. You should not use the measured torque values coming from your drive. From the data of earlier experiments it is clear that the firmware employs a low pass filter. For this drive assume the actual torque is equal to the setpoint and only 3-4-5 cycles delayed. With experimentation you can obtain the optimal value. This behavior is supported by the measured acceleration values.
2. You figured out in an indirect way that controlling a feedback torque at short timescales does not make sense. It is for that reason you have to set a maximal jerk value for all your control. This means that the rate of change for torque is limited between a positive and negative value. This will make your control system much more stable. Of course this also means you have to calculate your trajectory with jerk limits. Therefore I pointed you to libraries that already do that.

Running your control loop faster always makes sense. You just need to change your setpoints more smoothly. This improves mechanical behavior, but also makes the current controller in your drive more happy as it does not need to change current levels so fast.

[WillyTuring]

Hi Arthur,

From the data of earlier experiments it is clear that the firmware employs a low pass filter

1.True, it can be approximate as a first-order low pass filter system.

For this drive assume the actual torque is equal to the setpoint and only 3-4-5 cycles delayed. With experimentation you can obtain the optimal value.

2.The idea is not wrong, the problem is theoretically speaking the actual torque equal to torque command setpoint at 5*time constant approximately(In my case is 20 EtherCat cycles), i get the idea 3/4/5 here are just the illustration numbers, but "the delay of 20 EtherCat cycles" from the experiment is just too much, i'm not sure if it's still appropriate to compare a feedback value with a command setpoint 20 cycles ago.

It is for that reason you have to set a maximal jerk value for all your control. This means that the rate of change for torque is limited between a positive and negative value.

3.I actually set the jerk limitation mainly from the handling capability of the motor though.

This will make your control system much more stable. Of course this also means you have to calculate your trajectory with jerk limits. Therefore I pointed you to libraries that already do that.

4.Yes, i remember and i looked up the resources you pointed out, but sorry i didn't dig deep into it due to the fact that i want to get the most basic stuff done first, and from previous experience and simulation in ROS, i didn't have too much problem on the trajectory generation part. So i still put it on hold at this moment.

Running your control loop faster always makes sense. You just need to change your setpoints more smoothly.

5.This is for sure the best scenario, running a faster loop by updating every cycle is no doubt always a better measure assuming everything else is equal.

At this point, the EtherCat commucation cycle is running at 250Hz(0.004ms), while i'm updating setpoint every 10 EtherCat cycles so that gives me 25Hz, the updating frequency looks like much less than optimal on paper, but due to the fact that the time constant for this "low-pass filter system" is 4 EtherCat cycles, it reaches setpoint in 20 cycles on a bump test, my thinking is that a faster feedback loop rate can handle more feedforward prediction discrepancy from the dynamic model than a lower loop rate case. That's why when i recognized the difficulty level of running a faster loop rate due to the innate "time constant delay" from low-pass-filter-system to the setpoint command, i'm trying to use a more accurate feedforward model to compensate for the lower loop rate. So in my opinion, the best case scenario is definitely "An accurate feedforwad model + A faster feedback loop rate"; While "An accurate feedforward model + A slower feedback loop rate" ≈ "A less accurate feedforward model + A faster feedback loop rate".

Love to hear what are your thoughts on the above.

Edit: Forgot to mention one really important constraint that also pushed me away from updating the setpoint every EtherCat cycle, that is the previous observation we had on "Accceleration goes down" when "Velocity going up", this observation appears to be way much less accurate and clear when the data point is limited, meaning that even if i have the "Actual torque value" and the "Actual angular velocity", when we are only looking at a few cycles, the result yields from the model(in which the model developed from a large testing data set) might throw you off the roof, it's hard to correct even with feedback every cycle and the jerk limitation.

[ArthurKetels]

Thanks for the timing information. I must say your drive electronics have the worst response of any device I have ever seen. No wonder you have trouble controlling the motor. Are you sure the current loop tuning of your drive is optimized?

I have tested many different servo drives the past years. And more or less all of them have 95% setpoint reached after 2 or 3 cycles of PWM (at 16 to 32 kHz). EtherCAT packet delay is somewhere between zero and 2 cycles maximum.

If you have a current controller that is fast then the cascaded control loop works very well. When you have both latency and slow response it is very hard to get good control. My advice is to test again with another motor drive from a different manufacturer.

[WillyTuring]

Hi Arthur,

I must say your drive electronics have the worst response of any device I have ever seen.

1.I believe you have heard or might even worked with this driver before, it is Cooldrive-R6 from a Chinese company, because i remember the drive has some problem with EtherCat protocol conformity, like it didn't give out the feedback status from EtherCat packet immediately after reaching the OP mode, the feedback data only shown in the TPDO 200 cycles after it reaching the OP mode.

Are you sure the current loop tuning of your drive is optimized?

2.Well, i'm pretty sure the "P" and "I" coefficient was set not far from optimal, but not 100% confident. Here's what i've done with the current loop: the current loop is running on PI controller(No Derivative term), so it is in essence a second-order system, i was basically tuning it with experiments so that the coefficient i set with "P" and "I" term gives out a critical-damping response. The reason i thought it is a critical-damping response is from the experiments that if the "P" term is slightly higher, it causes the motor to jerk and unstable; if the "I" term is increased, it causes the response to overshoot, basically the response curve looks like a one from first-order system, though the main dinstinction between the two should be the slope from the starting point theoretically. So i thought those two coefficient was set not too far off.

I have tested many different servo drives the past years. And more or less all of them have 95% setpoint reached after 2 or 3 cycles of PWM (at 16 to 32 kHz). EtherCAT packet delay is somewhere between zero and 2 cycles maximum.

3.I must say this kind of response curve really makes me jealous.

My advice is to test again with another motor drive from a different manufacturer.

4.This motor drive is a 6-on-1 type meaning that the 6 drives are actually packed into one piece mechanically, although the motor can work with 3 or 4 different drive brand, but i'm not sure how do they work, and as far as i know at least from the marketing point of view, Cooldrive is actually a big-name in the drive industry......If it really behaves that bad compare to what you normally deal with, i guess i'll have to try to make out lemonade out of lemon for a while then. At least at this point, my feedforward model is doing a descent job, i guess that's the comforting point for me, i'll see what i can pull out from it.

[WillyTuring]

Hi Arthur, How are you doing. Like i told you last time, i was intending to construct a trapezoidal velocity curve(accelerating→constant-speed→decelerating-to-stop), i came up with some ideas and right now all three stages works fine, i'll post the result after some improvements.

But i have encountered a new problem which seems a little bit weird, that is for all the previous work, i was mainly focus on rotating the motor "forward"(meaning the pulse position increases), but as i was trying to control the motor move "backward"(meaning pulse position decreases), suddenly it didn't work, and later on i found out the model i used previously for feeding forward in "moving-forward situation" just wouldn't work on "moving-backward situation", basically the predicting result deviate quite a lot from the actual result, so i went back to test the model again with the "moving-forward situation", it works all fine.

I don't think it is the problem with the model itself by and large, because not only does it have a strong mechanical backbone, but also supported by control theory, my gut feeling tells me i am missing something while doing the back-ward rotation, so Arthur what do you think i'm missing here?

[WillyTuring]

Hi Arthur, Sorry for the wrong-doing of closing the thread. I think i solved the issue of the controlling the motor moving backward. Now i managed to rotate the motor forward and backward at any desired angle and speed in CST mode, though i'm still trying to improve the precision of stopping at the desired position.

Apart from this, now i'm doing the gravity compensation, in which my intention is that under CST mode, i need to move the link in any position and then read and save the position, after that i then use those position point as the waypoint of my trajectory.

So in order to do that, i need a "platform" in which i can run multiply "threads", one of them is to execute the command, and another is to read and save the data, and both of them need to be "online" all the time, so that i can change the position on the fly without change it in the code manually. Do you have any suggestion on this one?

[ArthurKetels]

Thanks for the updates on your controller. I can not give much feedback because you do not provide any details. So when you mention your controller did not work in backward direction I just have to take that as-is. If you do not provide the code I can not really help.

To build a controller "server" I recommend to use zeroMQ: https://zeromq.org/

The master with SOEM then runs as a real time task and communicates with the controller in an other executable. That in its turn communicates with your test application. Another task reads the data from SOEM and controller task and stores it in a database. All four can run independent, they can even run on multiple computers..

[WillyTuring]

Hi Arthur, Thanks for the recommendation. Sorry i didn't provide the code needed for debugging the previous problem, it's solely because i felt the code was a little bit messy. I definitely need to provide as much detail as needed for the help seeking.

I'm now looking up on ZeroMQ, seems pretty interesting, i was using ROS as my operating environment for this purpose, but i just feel like ROS is too heavy for my purpose, besides i am doing all the low-level control myself, i really don't need the packages(tools) provided by ROS(Though the simulation is good), so i will go with ZeroMQ first to try to build a communication platform, then worry about the simulation(visual displaying) part later.

Now a little bit quick info before the code and data: 1.My EtherCat communication cycle is 4ms. 2.I'm controlling the motor under "Trapezoidal velocity curve", acceleraing to a desired velocity, keeps running at the constant speed then decelerating till stop at the desired position. 3.I'm updating the target torque command in every 10 EtherCat cycles due to my time-constant is 4 EtherCat cycles, wasn't able to build a predictable relationship between Torque and Acceleration when updating the command in every EtherCat cycle. 4.Also wasn't able to use feedback appropriately which makes the control loop seems a little bit weak.

Arthur, i attached the complete code of mine and the running data(in which i ran the motor from 0 degrees to 180 degrees at a constant speed of 6.28rad/s), the data is raw data from printout shown in txt format, sorry if it looks rough.

Let me know what's your critique on my control principle, thanks in advance.

Code_and_DataCaptured.zip

[WillyTuring]

Hi Arthur, Hope you are doing well, i'm sure you are busying at your work, this is just a reminder message here in case you have some thoughts to share but forgot to do so.

By the way i'm digging into ZeroMQ for the last several days, seems pretty interesting, a lot of information though.

I'm looking forward to hear you back whenever you got time, take care!

[ArthurKetels]

Hi Willy,

I appreciate the work you put into this. But to be honest, the code is near impossible to follow. Also the screen output is not usable for analysis with plotting tools.

I do not have hours of time to dig into your code and finding out what is good and what is not. So at least could you provide me with a comma delimited file, where the first row is the header text and all the next lines are time steps? Take as many columns as you like, but keep them constant.

For the code, split it in logical functions.

• prepare and scale inputs (slave to SI units)
• Generate trajectory
• Calculate actual state (and compensate for delays)
• Calculate feedback from trajectory and current state
• Calculate future state
• Calculate feed forward from actual state and future state
• Calculate outputs from feed forward and feed back
• Clip and convert outputs (SI units to slave)

The code should be self explanatory without the need for comments. To clarify, state is all parameters that are needed to explain the process you want to control. Position, velocity, acceleration, torque, mass, inertia, gravity, friction, disturbance, etc.

[WillyTuring]

Hi Arthur, Yes you are right, the code does look a little bit messy from appearance, partly because of my inefficient and inelegant coding style, partly due to the entangled mathematic calculation from my exotic control principle.

And i'm sorry for all the previous long post, I have edited this post a few times before i sent hoping that i just cut my point to the bare necessity and minimum for you to read. data.xlsx

I guess my main question is HOW do you update your torque command every EtherCat cycle? This seems a little bit impossible for me, since torque is gerated from current, and current changes follows time constant(determined by resistor and inductor), unless you have a time constant τ = 1 EtherCat cycle, so you know 5 EtherCat cycles, the motor will reach to your setpoint, in this case "the general delay" is then a constant, updating every cycle is without a problem, otherwise i doubt the idea will work.

I suddenly realized does that mean if i could redue my current EtherCat cycle from 4ms to 1ms or even 250us, my time constant would be reduced accordingly? I will try that after i'm done with ZeroMQ in a few days.