I am trying to linearize a free-floating system with a free-floating base and 3 joints (j1, j2, j3). As I understand the positions part of the system state is given by the vector (this matches MultibodyPlant::num_positions()):
q (10x1) = [base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]
Since angular velocity requires only 3 components, the velocity part of the system state is written as (this matches MultibodyPlant::num_velocities()):
q_dot (9x1) = [base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]
Using this, the full system state is given as (this works when using MultibodyPlant::SetPositionsAndVelocities) :
X (19x1) = [q (10x1),q_dot (9x1)]
With this, the system acceleration resulting from its dynamics and control forces X_dot = f(X, U) would be written as:
X_dot (18x1)= [q_dot (9x1), q_ddot (9x1)]
Due to the difference in the representation of rotations and angular velocities, the number of terms needed to define X and X_dot is different.
This brings to the following questions while linearizing the system about a point using Linearize:
The A and B matrices after linearization of a continuous-time MultibodyPlant represent the equation X_dot = A*X + B*u. However, there seems to be a mismatch here in the sizes of the arrays/matrices involved as X_dot (18x1) is different from matrices given by Linearize: A (19x19) and B (19x3). I don't then understand what accelerations does the matrix X_dot from the linear system equation represents with its size 19x1?
The above question is only for a continuous-time case. For a discrete-time system,the following equations hold without any issues with the matrix sizes:X[n+1] = A_d * X[n] + B_d * u[n]. However, it is not clear how the quaternion properties are maintained during this linearized forward simulation?
I think there is misunderstanding in the notation since q_dot ≠ v.
Instead, q_dot is simply the ordinary time-derivative of q.
q (10x1) = [base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]
q_dot (10x1) = d/dt[base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]
Angular velocity only has 3 components, so v (the velocity part of the system state) and its time-derivative v_dot are:
v (9x1) = [base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]
v_dot (9x1) = d/dt[base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]
The full system state X and its time-derivative x_dot are shown below.
X (19x1) = [q (10x1), v ( 9x1)]
X_dot (19x1) = [q_dot (10x1), v_dot (9x1)]
Note: X ≠ [q, q_dot], instead X = [q, v].
Similarly, X_dot ≠ [q_dot, q_ddot], instead X = [q_dot, v_dot].
I am trying to formulate a trajectory optimization problem for a glider, where I want to maximize the average horisontal velocity. I have formulated the system as a drakesystem, and the state vector consists of the position and velocity.
Currently, I have something like the following:
dircol = DirectCollocation(
plant,
context,
num_time_samples=N,
minimum_timestep=min_dt,
maximum_timestep=max_dt,
)
... # other constraints etc
horisontal_pos = dircol.state()[0:2] # Only (x,y)
time = dircol.time()
dircol.AddFinalCost(-w.T.dot(horisontal_pos) / time)
where AddFinalCost() should replace all instances of state() and time() with the final values, as far as I understand from the documentation. min_dt is non-zero and w is a vector of linear weights.
However, I am getting the following error message
Expression (...) is not a polynomial. ParseCost does not support non-polynomial expression.
which makes me think that there is no way of adding the type of cost function that I am looking for. Is there anything that I am missing?
Thank you in advance!
When calling AddFinalCost(e) with e being a symbolic expression, we can only handle it when e is a polynomial function of the state (more precisely, either a quadratic function or a linear function). Hence the error you see complaining that the cost is not polynomial.
You could add the cost like this
def average_speed(v):
x = v[0]
time_steps = v[1:]
return v[0] / np.sum(time_steps)
h_vars = [dircol.timestep[i] for i in range(N-1)]
dircol.AddCost(average_speed, vars=[dircol.state(N-1)[0]] + h_vars)
which uses a function average_speed to evaluate the average speed. You could find example of doing this in https://github.com/RobotLocomotion/drake/blob/e5f3c3e5f7927ef675066d97d3afac55d3481305/bindings/pydrake/solvers/test/mathematicalprogram_test.py#L590
First, the cost function should be a scalar, but you a vector-valued horisontal_pos / time, which has two entries containing both position_x / dt and position_y / dt, namely a vector as the cost. You should instead provide a scalar valued cost.
Second, it is unclear to me why you divide time in the final cost. As far as I understand it, you want the final position to be close to the origin, so something like position_x² + position_y². The code can look like
dircol.AddFinalCost(horisontal_pos[0]**2 + horisontal_pos[1]**2)
I have long symbolic function in Maxima, let say
fn(x,y):=<<some long equation using x and y>>
I would like to calculate polynomial approximation of this function, let say
fn_poly(x,y)
within known range of x and y and with maximum error e
I know, that there is a funcionality in Maxima, e.g. plsquares, but it needs a matrix on input and I have only function fn(x,y). I don't know how to generate this matrix from my function. genmatrix creates matrix not usable by plsquares.
Is this possible in Maxima?
Make list of lists and transform it to matrix.
load(plsquares);
f(x,y):=x^2+y^3;
mat:makelist(makelist([X,Y,f(X,Y)],X,1,10,2),Y,1,10,2);
-> [[[1,1,2],[3,1,10],[5,1,26],[7,1,50],[9,1,82]],[[1,3,28],[3,3,36],[5,3,52],[7,3,76],[9,3,108]],[[1,5,126],[3,5,134],[5,5,150],[7,5,174],[9,5,206]],[[1,7,344],[3,7,352],[5,7,368],[7,7,392],[9,7,424]],[[1,9,730],[3,9,738],[5,9,754],[7,9,778],[9,9,810]]]
mat2:[];
for i:1 thru length(mat) do mat2:append(mat2,mat[i]);
mat3:funmake('matrix,mat2);
-> matrix([1,1,2],[3,1,10],[5,1,26],[7,1,50],[9,1,82],[1,3,28],[3,3,36],[5,3,52],[7,3,76],[9,3,108],[1,5,126],[3,5,134],[5,5,150],[7,5,174],[9,5,206],[1,7,344],[3,7,352],[5,7,368],[7,7,392],[9,7,424],[1,9,730],[3,9,738],[5,9,754],[7,9,778],[9,9,810])
ZZ:rhs(plsquares(mat3,[X,Y,Z],Z,3,3));
-> Determination Coefficient for Z = 1.0
-> Y^3+X^2
The F# language has the functions log which computes the natural logarithm and log10 which computes the base 10 logarithm.
What is the best way to compute a base 2 logarithm in F#?
Use System.Math.Log(number, base)
Example:
open System
Math.Log(32., 2.)
val it : float = 5.0
You could simply use the fact that the "a-logarithm of b" = ln(b) / ln(a), that is, the 2-logarithm of x is ln(x) / ln(2).
log2(8) = ln(8) / ln(2) = 3
log2(32) = ln(32) / ln(2) = 5
...where ln is either the natural logarithm or log10, either logarithm will work.
Since .NET 5.0 there is Math.Log2.
> Math.Log2 64;;
6
I'm a beginner in F#. I know that there is the way to create Double Matrix using PowerPack.dll:
let B = matrix [ [ 1.0; 7.0 ];
[ 1.0; 3.0 ] ]
How can I create matrix with elements of my own type (for example with [,] instead of Double), so it would look like:
let B = matrix [ [ [1,2]; [3,4] ];
[ [7,8]; [5,6] ] ]
I agree that matrix should be mainly used when working with numbers. The standard non-generic matrix type (which you can create using the matrix function) works with numbers of type float. If you want to work with other numeric types, you can use Matrix.Generic module, which contains functionality for working with generic matrices (containing any types).
You can use generic matrix for storing tuples as well (if you want). A generic matrix can be created using the ofList function. You can also define a function for this to get a nicer syntax:
let anymatrix = Matrix.Generic.ofList
let B = anymatrix [ [ [1,2]; [3,4] ];
[ [7,8]; [5,6] ] ]
To work with generic matrices, you can use the Matrix.Generic module:
let Bt = Matrix.Generic.transpose B
Typically, you'll use matrices only with numeric types, because many of the operations require some arithmetics in order to work. This will work for all basic numeric types (such as int) and you can also provide arithmetics for your own type using GlobalAssociations discussed here.
However, if you want to simply store some values then there are other (more suitable) data types. You can also use Array2D which represents a mutable two-dimensional array.
Tomas already gives a good answer. I will comment a bit here.
Let's see some source code in matrix.fsi from PowerPack:
type matrix = Matrix<float>
so matrix is a concrete type instantiated from the meta type Matrix. You could also use
type intmatrix = Matrix<int>
to define your int matrix type.
but to use something like:
let B = matrix [ [ 1.0; 7.0 ];
[ 1.0; 3.0 ] ]
We need another function called matrix, whose deceleration as
val matrix : seq<#seq<float>> -> matrix
let's see its implementation in matrix.fs:
let matrix ll = Microsoft.FSharp.Math.Matrix.ofSeq ll
while Microsoft.FSharp.Math.Matrix module is for double(in f# float) matrix, Microsoft.FSharp.Math.Matrix.Generics is for generic matrix. You can implement your intmatrix 'constructor'.
put it together:
type intmatrix = Matrix<int>
let intmatrix ll = Matrix.Generic.ofSeq ll
let C = intmatrix [ [1;2]; [3;4] ];
Matrix is a particularly mathematical type for working with numbers.
If you just want to arrange arbitrary elements in a rectangular shape, use standard F# lists or arrays.