Related
I'm trying to linearize a very simple model using OpenModelica's linearize() function, however, a translation error is produced likely because the equation of motion includes derivatives of the input variable.
Currently, I really don't mind the value of the input variable as long as I can get the state space representation with the function linearize(), from which I can calculate the transfer function. Therefore, I explicitly wrote the value of the input variable to be zero, but it still produces the error.
The definition of the model and the output generated in OMShell are below.
Any help would be appreciated.
Regards,
Fabian
Model definition:
model IP_v0
// Input and output variables
output Real x_load (start = 100e-6) "Displacement of the load";
input Real x_ground = 0 " Displacement of the ground";
// Velocities
Real v_load (start = 0) "Velocity of the load";
Real v_ground (start = 0) "Velocity of the ground";
// Parameters of the equation of motion
parameter Real beta = -1e-4;
parameter Real omega = 2 * 3.141592653 * 0.100 "Frequency of 100 mHz" ;
equation
v_load = der(x_load);
v_ground = der(x_ground);
der(v_load) + beta * der(v_ground) = -omega ^ 2 * (x_load - x_ground);
end IP_v0;
OMShell session:
linearize(VIS.IP_v0)
record SimulationResult
resultFile = "",
simulationOptions = "startTime = 0.0, stopTime = 1.0, numberOfIntervals = 500, tolerance = 1e-06, method = 'dassl', fileNamePrefix = 'VIS.IP_v0', options = '', outputFormat = 'mat', variableFilter = '.*', cflags = '', simflags = ''",
messages = "Failed to run the linearize command: VIS.IP_v0",
timeFrontend = 0.0041959,
timeBackend = 0.0,
timeSimCode = 0.0,
timeTemplates = 0.0,
timeCompile = 0.0,
timeSimulation = 0.0,
timeTotal = 0.0042162
end SimulationResult;
getErrorString()
"Error: Internal error - IndexReduction.pantelidesIndexReductionMSS failed! Use -d=bltdump to get more information.
Error: Internal error - IndexReduction.pantelidesIndexReduction1 failed! Use -d=bltdump to get more information.
Error: Internal error - IndexReduction.pantelidesIndexReduction failed!
Error: Internal error Transformation Module PFPlusExt index Reduction Method Pantelides failed!
I want to turn the position around the TCP like in jog mode does
The first try was to add the value to the position:
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
e6pOffsetPos.B = e6pOffsetPos.B + 50
PTP e6pOffsetPos C_DIS
Then I tried the geometric operator ":"
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
f = {X 0.0, Y 0.0, Z 0.0, A 0.0, B 50.0, C 0.0}
PTP e6pOffsetPos:f C_DIS
TOOL_DATA[1]={X -22.5370,Y 145.857,Z 177.617,A 0.0,B 0.0,C 0.0}
somehow I know that the geometric operator works if I change A, B, C of $TOOL correctly. Direction to the grap object. That means a diferent orientation does need other A, B, C of $TOOL and its not very intuitive to get it...
is there a easier way to do this or to understand this?
You are really close! KUKA uses the Euler ZYX system to calculate the TCP orientation. This means that three rotations happen in a specific sequence to achieve the final orientation. The order is:
A rotation about the world Z axis
B rotation about the new Y axis
C rotation about the new X axis
hence, Euler ZYX.
In order to do a rotation, similar to how TOOL TCP jog operates, you need to do a frame transformation from your current position to the target position. If you want to rotate about the current B axis, it takes more than just a B adjustment of POS_ACT to get there, because the B rotation is only a part of a sequence of rotations where you end up.
So our main program should have some code like this:
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
current_pos = $POS_ACT
offset = {X 0.0, Y 0.0, Z 0.0, A 0.0, B 50.0, C 0.0}
new_pos = transform_frame(offset, current_pos)
Now lets write code to support that function. I make a math_library.src:
DEF math_library()
END
GLOBAL DEFFCT FRAME transform_frame (offset:IN, origin:IN)
DECL FRAME offset, result_frame, origin
DECL REAL rot_coeff[3,3], final[3,3], reverse[3,3]
rot_to_mat(rot_coeff[,], origin.A, origin.B, origin.C)
result_frame.X = rot_coeff[1,1]*offset.X+rot_coeff[1,2]*offset.Y+rot_coeff[1,3]*offset.Z+origin.X
result_frame.Y = rot_coeff[2,1]*offset.X+rot_coeff[2,2]*offset.Y+rot_coeff[2,3]*offset.Z+origin.Y
result_frame.Z = rot_coeff[3,1]*offset.X+rot_coeff[3,2]*offset.Y+rot_coeff[3,3]*offset.Z+origin.Z
rot_to_mat(reverse[,], offset.A, offset.B, offset.C)
mat_mult(final[,], rot_coeff[,], reverse[,])
mat_to_rot(final[,], result_frame.A, result_frame.B, result_frame.C)
return result_frame
ENDFCT
GLOBAL DEF rot_to_mat(t[,]:OUT,a :IN,b :IN,c :IN )
;Conversion of ROT angles A, B, C into a rotation matrix T
;T = Rot_z (A) * Rot_y (B) * Rot_x (C)
;not made by me. This was in KEUWEG2 function
REAL t[,], a, b, c
REAL cos_a, sin_a, cos_b, sin_b, cos_c, sin_c
cos_a=COS(a)
sin_a=SIN(a)
cos_b=COS(b)
sin_b=SIN(b)
cos_c=COS(c)
sin_c=SIN(c)
t[1,1] = cos_a*cos_b
t[1,2] = -sin_a*cos_c + cos_a*sin_b*sin_c
t[1,3] = sin_a*sin_c + cos_a*sin_b*cos_c
t[2,1] = sin_a*cos_b
t[2,2] = cos_a*cos_c + sin_a*sin_b*sin_c
t[2,3] = -cos_a*sin_c + sin_a*sin_b*cos_c
t[3,1] = -sin_b
t[3,2] = cos_b*sin_c
t[3,3] = cos_b*cos_c
END
GLOBAL DEF mat_to_rot (t[,]:OUT, a:OUT, b:OUT, c:OUT)
;Conversion of a rotation matrix T into the angles A, B, C
;T = Rot_z(A) * Rot_y(B) * Rot_x(C)
;not made by me. This was in KEUWEG2 function
REAL t[,], a, b, c
REAL sin_a, cos_a, sin_b, abs_cos_b, sin_c, cos_c
a = ARCTAN2(t[2,1], t[1,1])
sin_a = SIN(a)
cos_a = COS(a)
sin_b = -t[3,1]
abs_cos_b = cos_a*t[1,1] + sin_a*t[2,1]
b = ARCTAN2(sin_b, abs_cos_b)
sin_c = sin_a*t[1,3] - cos_a*t[2,3]
cos_c = -sin_a*t[1,2] + cos_a*t[2,2]
c = ARCTAN2(sin_c, cos_c)
END
GLOBAL DEF mat_mult (a[,]:OUT,b[,]:OUT,c[,]:OUT)
DECL REAL a[,], b[,], c[,]
DECL INT i, j
;multiply two 3x3 matrices
FOR i = 1 TO 3
FOR j = 1 TO 3
a[i, j] = b[i,1]*c[1,j] + b[i,2]*c[2,j] + b[i,3]*c[3,j]
ENDFOR
ENDFOR
END
mat_to_rot, and rot_to_mat are used to convert between a 3x3 rotation matrix and A,B,C angles. I won't go into detail about rotation matrices, but they are fundamental for doing rotation math like this. I know this is a huge mouthful, and there are probably better ways, but I've never had any regrets adding this code to a global math library and throwing it on my robots just in case.
Is the arctan2-function from KUE_WEG like this?
DEFFCT REAL ARCTAN2 (Y: IN, X: IN)
; Arcustangens mit 2 Argumenten und Check, ob x und y numerisch 0 sind
REAL X, Y
REAL ATAN_EPS
ATAN_EPS = 0.00011
IF ( (ABS(X) < ATAN_EPS) AND (ABS(Y) < ATAN_EPS) ) THEN
RETURN (0)
ELSE
RETURN ( ATAN2(Y, X) )
ENDIF
ENDFCT
But i cannot found anything like mat_mult(final[,], rot_coeff[,], reverse[,]).
Would be great if you can complete this.
Big thanks to you
I'm using the curve fitting app in MATLAB. If I understand correctly the "b1" component in the left box is the mean of function i.e. the x point where y=50% and my x data is [-0.8 -0.7 -0.5 0 0.3 0.5 0.7], so why is this number in this example so big (631)?
General model Gauss1:
f(x) = a1*exp(-((x-b1)/c1)^2)
Coefficients (with 95% confidence bounds):
a1 = 3.862e+258 (-Inf, Inf)
b1 = 631.2 (-1.117e+06, 1.119e+06)
c1 = 25.83 (-2.287e+04, 2.292e+04)
Your data looks like cdf and not pdf. You can use this code for your solution
xi=[-0.8,-0.7,-0.5, 0.0, 0.3, 0.5, 0.7];
yi= [0.2, 0.0, 0.2, 0.2, 0.5, 1.0, 1.0];
fun=#(v) normcdf(xi,v(1),v(2))-yi;
[v]=lsqnonlin(fun,[1,1]); %[1,2]
mu=v(1); sigma=v(2);
x=linspace(-1.5,1.5,100);
y=normcdf(x,mu,sigma);
figure(1);clf;plot(xi,yi,'x',x,y);
annotation('textbox',[0.2,0.7,0.1,0.1], 'String',sprintf('mu=%f\nsigma=%f',mu,sigma),'FitBoxToText','on','FontSize',16);
you will get: mu=0.24537, sigma=0.213
And if you still want to fit to pdf, just change the function 'normcdf' in 'fun' (and 'y') to 'normpdf'.
To be precise, the loss function that I'm looking for is the squared error when the absolute error is lesser than 0.5, and it is the absolute error itself, when the absolute error is greater than 0.5. In this way, the gradient from the error function doesn't exceed 1 because once the gradient of the squared error function reaches 1, the absolute error function kicks in, and the gradient remains constant at 1. I've included my current implementation below. For some reason, it's giving me worse performance than just the squared error.
fn_choice_maker1 = (tf.to_int32(tf.sign(y - y_ + 0.5)) + 1)/2
fn_choice_maker2 = (tf.to_int32(tf.sign(y_ - y + 0.5)) + 1)/2
choice_maker_sqr = tf.to_float(tf.mul(fn_choice_maker1, fn_choice_maker2))
sqr_contrib = tf.mul(choice_maker_sqr, tf.square(y - y_))
abs_contrib = tf.abs(y - y_)-0.25 - tf.mul(choice_maker_sqr, tf.abs(y - y_)-0.25)
loss = tf.reduce_mean(sqr_contrib + abs_contrib)
train_step = tf.train.AdamOptimizer(1e-4).minimize(loss)
choice_maker_sqr is a column tensor that is one whenever the error is between 0.5 and -0.5. The names are pretty self explanatory.
Here is my implementation of the Huber loss function in python tensorflow:
def huber_loss(y_true, y_pred, max_grad=1.):
"""Calculates the huber loss.
Parameters
----------
y_true: np.array, tf.Tensor
Target value.
y_pred: np.array, tf.Tensor
Predicted value.
max_grad: float, optional
Positive floating point value. Represents the maximum possible
gradient magnitude.
Returns
-------
tf.Tensor
The huber loss.
"""
err = tf.abs(y_true - y_pred, name='abs')
mg = tf.constant(max_grad, name='max_grad')
lin = mg*(err-.5*mg)
quad=.5*err*err
return tf.where(err < mg, quad, lin)
You can use tf.select to implement it in a single call:
err = y - y_
huber_loss = tf.select(tf.abs(err) < 1.0,
0.5 * tf.square(err),
tf.abs(err) - 0.5) # if, then, else
err = tf.subtract(x,y)
huber_loss = tf.where(tf.less(x,y),
tf.sqrt(tf.square(err)),
tf.abs(err))
with tf.Session() as sess:
print(sess.run(tf.reduce_mean(huber_loss)))
Not sure if this is still relevant, but I would like to point it out to those seeking this in the future. The tensorflow research losses script has an implementation of the Huber loss for Object detection (like its implemented in the FasterRCNN paper)
Here's the link to the method
I am running OpenModelica and trying to run an example from Introduction to Physical Modeling with Modelica. I have copied examples 9.1 - 9.4 into a package. The file now looks like this:
package gravityPackage
//Test of gravity taken from Intro to Physical modeling with Modelica
//
//
//
//
//
model ParticleField
inner function gravity = TwoBodyField;
Particle p1(x_init = {2,-2,0}, v_init = {0.7,0,0});
Particle p2(x_init = {0,0.5,0}, v_init = {-1,-1,0});
Particle p3(x_init = {0.5,2,0}, v_init = {-1,-0.5,0});
end ParticleField;
function TwoBodyField
extends GravityField;
protected
Modelica.SIunits.Position b1[3],b2[3];
Modelica.SIunits.Velocity n1[3],n2[3];
algorithm
b1:={0,0,0};
b2:={0,1,0};
n1:=-(x - b1) / sqrt((x - b1) * (x - b1));
n2:=-(x - b2) / sqrt((x - b2) * (x - b2));
g:=n1 / ((x - b1) * (x - b1)) + n2 / ((x - b2) * (x - b2));
end TwoBodyField;
partial function GravityField
input Modelica.SIunits.Position x[3];
output Modelica.SIunits.Acceleration g[3];
end GravityField;
model Particle
parameter Modelica.SIunits.Position x_init[3];
parameter Modelica.SIunits.Velocity v_init[3];
protected
outer function gravity = GravityField;
//outer function gravity=ParticleField;
//outer function gravity=TwoBodyField;
Modelica.SIunits.Position x[3](start = x_init);
Modelica.SIunits.Velocity v[3](start = v_init);
Modelica.SIunits.Acceleration a[3];
equation
v = der(x);
a = der(v);
a = gravity(x);
end Particle;
end gravityPackage;
But, if I go to OMShell and try to run it, I get this:
>> loadFile("gravityPackage.mo")
true
>> simulate(gravityPackage.ParticleField)
record SimulationResult
resultFile = "",
simulationOptions = "startTime = 0.0, stopTime = 1.0, numberOfIntervals = 500, tolerance = 0.000001, method = 'dassl', fileNamePrefix = 'gravityPackage.ParticleField', options = '', outputFormat = 'mat', variableFilter = '.*', measureTime = false, cflags = '', simflags = ''",
messages = "Simulation failed for model: gravityPackage.ParticleField
[gravityPackage.mo:34:11-34:42:writable] Warning: No corresponding 'inner' declaration found for class gravity declared as 'outer '.
Continuing flattening by only considering the 'outer' class declaration.
[gravityPackage.mo:43:5-43:19:writable] Error: Failed to instantiate equation
a = gravity(x);.
Error: Error occurred while flattening model gravityPackage.ParticleField
",
timeFrontend = 0.0,
timeBackend = 0.0,
timeSimCode = 0.0,
timeTemplates = 0.0,
timeCompile = 0.0,
timeSimulation = 0.0,
timeTotal = 0.0
end SimulationResult;
>>
So, clearly there is something related to scope that I am not getting correct. All the code, except for the package, is just copied directly from the book. I believe that the package is necessary to get it into a single file (although I tried a few other ways to do that with no success).
Any suggestions are appreciated.
Thanks,
This is a bug in OpenModelica.
It should work fine if is a inner component or class but not for functions.
I added a bug report about it and we will fix it:
https://trac.openmodelica.org/OpenModelica/ticket/2467
For now you can use an inner/outer package, that should work fine.
package gravityPackage
package Functions
function TwoBodyField
extends GravityField;
protected
Modelica.SIunits.Position b1[3],b2[3];
Modelica.SIunits.Velocity n1[3],n2[3];
algorithm
b1:={0,0,0};
b2:={0,1,0};
n1:=-(x - b1) / sqrt((x - b1) * (x - b1));
n2:=-(x - b2) / sqrt((x - b2) * (x - b2));
g:=n1 / ((x - b1) * (x - b1)) + n2 / ((x - b2) * (x - b2));
end TwoBodyField;
partial function GravityField
input Modelica.SIunits.Position x[3];
output Modelica.SIunits.Acceleration g[3];
end GravityField;
end Functions;
model ParticleField
inner package funcs = Functions;
Particle p1(x_init = {2,-2,0}, v_init = {0.7,0,0});
Particle p2(x_init = {0,0.5,0}, v_init = {-1,-1,0});
Particle p3(x_init = {0.5,2,0}, v_init = {-1,-0.5,0});
end ParticleField;
model Particle
parameter Modelica.SIunits.Position x_init[3];
parameter Modelica.SIunits.Velocity v_init[3];
protected
outer package funcs = Functions;
function gravity = funcs.TwoBodyField;
//outer function gravity=ParticleField;
//outer function gravity=TwoBodyField;
Modelica.SIunits.Position x[3](start = x_init);
Modelica.SIunits.Velocity v[3](start = v_init);
Modelica.SIunits.Acceleration a[3];
equation
v = der(x);
a = der(v);
a = gravity(x);
end Particle;
end gravityPackage;
OK, so I think the issue here is that you are trying to "pass" a function up the instance tree using dynamic scoping (i.e. inner and outer). I haven't read the specification on this particular point, but I don't think this will work. The reason is that normally inner and outer are used in conjunction with things that are instantiated (variables, models, etc).
So the thing to understand is that conceptually both constant variables and function definitions are not instantiated. Being able to treat a function as an instance (a first-class value) has several useful consequences but it also introduces some semantic complications. There has been work done in recent versions of the language toward trying to treat functions as first class citizens. The main use case was to pass functions as arguments to other functions. But I think these new semantics stop short of making functions truly first class values (as your example would demonstrate). However, this may be in issue with OpenModelica. I can't say definitively.
One way you could deal with this would be to use replaceable and redeclare. So in your Particle model, define gravity like this:
public
replaceable function gravity = GravityField;
Then, instantiate it as follows:
model ParticleField
Particle p1(x_init = {2,-2,0}, v_init = {0.7,0,0}, redeclare function gravity = TwoBodyField);
Particle p2(x_init = {0,0.5,0}, v_init = {-1,-1,0}, redeclare function gravity = TwoBodyField);
Particle p3(x_init = {0.5,2,0}, v_init = {-1,-0.5,0}, redeclare function gravity = TwoBodyField);
end ParticleField;
Note, I haven't tested this solution. For example, your Particle model might require the partial qualifier (since you would have to override gravity to a non-partial implementation).
I hope that helps.