I am trying to implement Non Linear MPC for a 7-DOF manipulator in drake. To do this, in my constraints, I need to have dynamic parameters like the Mass matrix M(q) and the bias term C(q,q_dot)*q_dot, but those depend on the decision variables q, q_dot.
I tried the following
// finalize plant
// create builder, diagram, context, plant context
...
// formulate optimazation problem
drake::solvers::MathematicalProgram prog;
// create decision variables
...
std::vector<drake::solvers::VectorXDecisionVariable> q_v;
std::vector<drake::solvers::VectorXDecisionVariable> q_ddot;
for (int i = 0; i < H; i++) {
q_v.push_back(prog.NewContinuousVariables<14>(state_var_name));
q_ddot.push_back(prog.NewContinuousVariables<7>(input_var_name));
}
// add cost
...
// add constraints
...
for (int i = 0; i < H; i++) {
plant.SetPositionsAndVelocities(*plant_context, q_v[i]);
plant.CalcMassMatrix(*plant_context, M);
plant.CalcBiasTerm(*plant_context, C_q_dot);
}
...
for (int i = 0; i < H; i++) {
prog.AddConstraint( M * q_ddot[i] + C_q_dot + G >= lb );
prog.AddConstraint( M * q_ddot[i] + C_q_dot + G <= ub );
}
// solve prog
...
The above code will not work, because plant.SetPositionsAndVelocities(.) doesn't accept symbolic variables.
Is there any way to integrate M,C in my ocp constraints ?
I think you want to impose the following nonlinear nonconvex constraint
lb <= M * qddot + C(q, v) + g(q) <= ub
This constraint is non-convex. We will need to solve it through nonlinear optimization, and evaluate the constraint in every iteration of the nonlinear optimization. We can't do this evaluation using symbolic computation (it would be horribly slow with symbolic computation).
So you will need a constraint evaluator, something like this
// This constraint takes [q;v;vdot] and evaluate
// M * vdot + C(q, v) + g(q)
class MyConstraint : public solvers::Constraint {
public:
MyConstraint(const MultibodyPlant<AutoDiffXd>& plant, systems::Context<AutoDiffXd>* context, const Eigen::Ref<const Eigen::VectorXd>& lb, const Eigen::Ref<const Eigen::VectorXd>& ub) : solvers::Constraint(plant.num_velocitiex(), plant.num_positions() + 2 * plant.num_velocities(), lb, ub), plant_{plant}, context_{context} {
...
}
private:
void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x, AutoDiffVecXd* y) const {
...
}
MultibodyPlant<AutoDiffXd> plant_;
systems::Context<AutoDiffXd>* context_;
};
int main() {
...
// Construct the constraint and add it to every time instances
std::vector<std::unique_ptr<systems::Context<AutoDiffXd>>> plant_contexts;
for (int i = 0; i < H; ++i) {
plant_contexts.push_back(plant.CreateDefaultContext());
prog.AddConstraint(std::make_shared<MyConstraint>(plant, plant_context[i], lb, ub), {q_v[i], qddot[i]});
}
}
You could refer to the class CentroidalMomentumConstraint on how to construct your own MyConstraint class.
Related
I am interfacing some code with raw pointers. So I have extensive use of the map class:
void foo(T* raw_pointer){
const int rows = ...;
const int cols = ...;
Map<Matrix<T, rows, cols>> mat(raw_pointer);
// DO some stuff with "mat"
}
Now I want to apply some cwise operations in foo, which I accomplish using .array(). The code works, however, it looks very messy due to all of the .array() calls strewn in the function. For instance, for the sake of argument, let's suppose that the function looked like this:
void foo(T* raw_pointer){
const int rows = ...;
const int cols = ...;
Map<Matrix<T, rows, cols>> mat(raw_pointer);
for (int i = 0 ; i < 1000 ; ++i)
... something = i * mat.row(1).array() * sin(mat.row(4).array()) + mat.col(1).array();
}
Part of the problem with this is that it is very unclear what the code is actually doing. It would be much nicer if gave the variables names:
void foo(T* raw_pointer){
const int rows = ...;
const int cols = ...;
Map<Matrix<T, rows, cols>> mat(raw_pointer);
Matrix<T, 1, cols> thrust = mat.row(1);
Matrix<T, 1, cols> psi = mat.row(4);
Matrix<T, 1, cols> bias = mat.row(2);
for (int i = 0 ; i < 1000 ; ++i)
... something = i * thrust.array() * sin(psi.array()) + bias.array();
}
But it would be even nicer if I could get directly get a reference to the ArrayWrappers so that we aren't making any copies. However, the only way I can figure out how to get that to work is by using auto:
void foo(T* raw_pointer){
const int rows = ...;
const int cols = ...;
Map<Matrix<T, rows, cols>> mat(raw_pointer);
auto thrust = mat.row(1).array();
auto psi = mat.row(4).array();
auto bias = mat.row(2).array();
for (int i = 0 ; i < 1000 ; ++i)
... something = i * thrust * sin(psi) + bias;
}
This code works, and upon testing appears to reference the entries in the pointer (as opposed to making copies like in the previous snippet). However,
I am concerned about its efficiency since the Eigen documentation explicitly suggests NOT doing this. So could somebody please what the preferred way to define the types for the variables is in such a circumstance?
It seems to me like I should be using a Ref here, but I can't figure out how to get that to work. Specifically, I have tried replacing auto with
Eigen::Ref<Eigen::Array<T, 1, cols>>
and
Eigen::Ref<Eigen::ArrayWrapper<Eigen::Matrix<T, 1, cols>>>
but the compiler doesn't like either of those.
To avoid having to write array() every time you use the Map<Eigen::Matrix... you can use a Map<Eigen::Array... instead/in addition. This will use the default element-wise operators instead of the matrix operators. To use a matrix operator instead, you can use map.matrix() (similar to what you have in your post mat.array()).
auto thrust = [](auto&&mat){return mat.row(1).array();};
auto psi = [](auto&&mat){return mat.row(4).array();};
auto bias = [](auto&&mat){return mat.row(2).array();};
for (int i = 0 ; i < 1000 ; ++i)
... something = i * thrust(mat) * sin(psi(mat)) + bias(mat)
has names. And the array wrappers don't persist.
I wrote a very simple function in GSL, to select a submatrix from an existing matrix in a struct.
EDIT: I had timed VERY INCORRECTLY and didn't notice the changed number of zeros in front.Still, I hope this can be sped up
For 100x100 submatrices of a 10000x10000 matrix, it takes 1.2E-5 seconds. So, repeating that 1E4 times, takes 50 times longer than I need to diagonalise the 100x100 matrix.
EDIT:
I realise, it happens even if I comment out everything except return(0);
Thus, I theorize, it must be something about struct TOWER. This is how TOWER looks:
struct TOWER
{
int array_level[TOWERSIZE];
int array_window[TOWERSIZE];
gsl_matrix *matrix_ordered_covariance;
gsl_matrix *matrix_peano_covariance;
double array_angle_tw[XISTEP];
double array_correl_tw[XISTEP];
gsl_interp_accel *acc_correl; // interpolating for correlation
gsl_spline *spline_correl;
double array_all_eigenvalues[TOWERSIZE]; //contains all eiv. of whole matrix
std::vector< std::vector<double> > cropped_peano_covariance, peano_mask;
};
Below comes my function!
/* --- --- */
int monolevelsubmatrix(int i, int j, struct TOWER *tower, gsl_matrix *result) //relying on spline!! //must addd auto vanishing
{
int firstrow, firstcol,mu,nu,a,b;
double aux, correl;
firstrow = helix*i;
firstcol = helix*j;
gsl_matrix_view Xi = gsl_matrix_submatrix (tower ->matrix_ordered_covariance, firstrow, firstcol, helix, helix);
gsl_matrix_memcpy (result, &(Xi.matrix));
return(0);
}
/* --- --- */
The problem is almost certainly gls_matric_memcpy. The source for that is in copy_source.c, with:
const size_t src_tda = src->tda ;
const size_t dest_tda = dest->tda ;
size_t i, j;
for (i = 0; i < src_size1 ; i++)
{
for (j = 0; j < MULTIPLICITY * src_size2; j++)
{
dest->data[MULTIPLICITY * dest_tda * i + j]
= src->data[MULTIPLICITY * src_tda * i + j];
}
}
This would be quite slow. Note that gls_matrix_memcpy returns a GLS_ERROR if the matrices are different sizes, so it's very likely the data member could be served with a CRT memcpy on the data members of dest and src.
This loop is very slow. Each cell is derefence through dest & src structs for the data member, and THEN indexed.
You could choose to write a replacement for the library, or write your own personal version of this matrix copy, with something like (untested suggestion code here):
unsigned int cellsize = sizeof( src->data[0] ); // just psuedocode here
memcpy( dest->data, src->data, cellsize * src_size1 * src_size2 * MULTIPLICITY )
Note that MULTIPLICITY is a define, usually 1 or 2, probably depends on library configuration - might not apply to your usage (if it's 1 )
Now, important caveat....if the source matrix is a subview, then you have to go by rows...that is, a loop of rows in i where crt's memcpy is limited to rows at a time, not the entire matrix as I show above.
In other words, you do have to account for the source matrix geometry from which the subview was taken...that's probably why they index each cell (makes it simple).
If, however, you KNOW the geometry, you can very likely optimize this WAY above the performance you're seeing.
If all you did was take out the src/dest derefence, you'd see SOME performance gain, as in:
const size_t src_tda = src->tda ;
const size_t dest_tda = dest->tda ;
size_t i, j;
float * dest_data = dest->data; // psuedocode here
float * src_data = src->data; // psuedocode here
for (i = 0; i < src_size1 ; i++)
{
for (j = 0; j < MULTIPLICITY * src_size2; j++)
{
dest_data[MULTIPLICITY * dest_tda * i + j]
= src_data[MULTIPLICITY * src_tda * i + j];
}
}
We'd HOPE the compiler recognized that anyway, but...sometimes...
This question already has answers here:
Fastest way to clamp a real (fixed/floating point) value?
(14 answers)
Closed 7 years ago.
I want to create a simple struct that stores the RGB-values of a color. r, g and b are supposed to be double numbers in [0,1].
struct Color
{
Color(double x): r{x}, g{x}, b{x} {
if (r < 0.0) r = 0.0;
if (r > 1.0) r = 1.0;
if (g < 0.0) g = 0.0;
if (g > 1.0) g = 1.0;
if (b < 0.0) b = 0.0;
if (b > 1.0) b = 1.0;
}
}
Is there a better way than using those if statements?
Just write a function to clamp:
double clamp(double val, double left = 0.0, double right = 1.0) {
return std::min(std::max(val, left), right);
}
And use that in your constructor:
Color(double x)
: r{clamp(x)}
, g{clamp(x)}
, b{clamp(x)}
{ }
You, can can use min and max, ideally combining them into a clamp function:
template <class T>
T clamp(T val, T min, T max)
{
return std::min(max, std::max(min, val));
}
struct Color
{
Color(double x) : r{clamp(x, 0., 1.)}, g{clamp(x, 0., 1.)}, b{clamp(x, 0., 1.)}
{}
};
For a first pass iteration, we have min/max functions we can and should use:
struct Color
{
explicit Color(double x): r{x}, g{x}, b{x}
{
r = std::max(r, 0.0);
r = std::min(r, 1.0);
g = std::max(g, 0.0);
g = std::min(g, 1.0);
b = std::max(b, 0.0);
b = std::min(b, 1.0);
}
double r, g, b;
};
I'd also suggest making that constructor explicit, as it's rather confusing for a scalar to implicitly convert to a Color.
The reason this is arguably an upgrade even with roughly the same amount of code and arguably not the biggest improvement in readability is because, while optimizing compilers might emit faster branchless code here, min and max can often guarantee an efficient implementation. You're also expressing what you're doing in a slightly more direct way.
There is some truth to this somewhat counter-intuitive idea that writing higher level code helps you achieve efficiency, if only for the reason that the low-level logic used to implement the high-level function is more likely to be efficient than what people would repeatedly write otherwise in their more casual, daily kind of code. It also helps direct your codebase towards more central targets for optimization.
As a second pass, this may not improve things for your particular use cases, but in general I've found it's useful to represent color and vector components using an array to allow you to access them with loops. This is because if you start doing somewhat complex things with colors like blending them together, the logic for each color component is non-trivial but identical for all components, so you don't want to end up writing such code three times all the time or always be forced into writing the per-component logic in a separate function or anything like that.
So we might do this:
class Color
{
public:
explicit Color(double x)
{
for (int j=0; j < 3; ++j)
{
rgb[j] = x;
rgb[j] = std::max(rgb[j], 0.0);
rgb[j] = std::min(rgb[j], 1.0);
}
}
// Bounds-checking assertions in these would also be a nice idea.
double& operator[](int n) {return rgb[n]};
double operator[](int n) const {return rgb[n]};
double& red() {return rgb[0];}
double red() const {return rgb[0];}
double& green() {return rgb[1];}
double green() const {return rgb[1];}
double& blue() {return rgb[2];}
double blue() const {return rgb[2];}
// Somewhat excess fluff, but such methods can be useful when
// interacting with a low-level C-style API (OpenGL, e.g.) as
// opposed to using &color.red() or &color[0].
double* data() {return rgb;}
const double* data() const {return rgb;}
private:
double rgb[3];
};
Finally, as others have mentioned, this is where a function to clamp values to a range is useful, so as a final pass:
template <class T>
T clamp(T val, T low, T high)
{
assert(low <= high);
return std::max(std::min(val, high), low);
}
// New constructor using clamp:
explicit Color(double x)
{
for (int j=0; j < 3; ++j)
rgb[j] = clamp(x, 0.0, 1.0);
}
I am getting this error in trying to implement the Bezier Curve psuedocode in C++ in Qt , have the method implementation below.
void GLWidget::drawBezierCurve() {
int N_PTS = vertices.size();
Point bezPoints[N_PTS][N_PTS];
for (float u = 0.0; u <= 1.0; u += 0.01){
for(int diag = N_PTS/2; diag >= 0;diag--){
for(int i = 0; i <= diag; i++){
int j = diag - i;
bezPoints[i][j] = (1.0 - u) * bezPoints[i][j+1] + u * bezPoints[i+1][j];
}
}
theImage.setPixel(bezPoints[0][0], bezPoints[0][0], RGBValue(100,12,140), 255);
}
}
This looks like it is because you are multiplying a float by a Point object. You are most likely going to need to define you're own multiplication method for this operation, or overload the * operator to perform this, depending on which fields in the Point object you intend to multiply the floating point number by.
Something like:
float operator* (const float num, const Point& point) {
return num * point.floating_point_field;
}
Where the floating_point_field is the member of the class that you want to multiply and it should also be of the same type as float, otherwise you'll have to start doing something more involved to define the multiplication.
Alternatively, if the multiplication is as simple as in the example above you could just use a getter in the code such as:
u * bezPoints[i][j+1].get_floating_point_value()
Hope that helps,
Matt
The problem to solve is finding the floating status of a floating body, given its weight and the center of gravity.
The function i use calculates the displaced volume and center of bouyance of the body given sinkage, heel and trim.
Where sinkage is a length unit and heel/trim is an angle limited to a value from -90 to 90.
The floating status is found when displaced volum is equal to weight and the center of gravity is in a vertical line with center of bouancy.
I have this implemeted as a non-linear Newton-Raphson root finding problem with 3 variables (sinkage, trim, heel) and 3 equations.
This method works, but needs good initial guesses. So I am hoping to find either a better approach for this, or a good method to find the initial values.
Below is the code for the newton and jacobian algorithm used for the Newton-Raphson iteration. The function volume takes the parameters sinkage, heel and trim. And returns volume, and the coordinates for center of bouyancy.
I also included the maxabs and GSolve2 algorithms, I belive these are taken from Numerical Recipies.
void jacobian(float x[], float weight, float vcg, float tcg, float lcg, float jac[][3], float f0[]) {
float h = 0.0001f;
float temp;
float j_volume, j_vcb, j_lcb, j_tcb;
float f1[3];
volume(x[0], x[1], x[2], j_volume, j_lcb, j_vcb, j_tcb);
f0[0] = j_volume-weight;
f0[1] = j_tcb-tcg;
f0[2] = j_lcb-lcg;
for (int i=0;i<3;i++) {
temp = x[i];
x[i] = temp + h;
volume(x[0], x[1], x[2], j_volume, j_lcb, j_vcb, j_tcb);
f1[0] = j_volume-weight;
f1[1] = j_tcb-tcg;
f1[2] = j_lcb-lcg;
x[i] = temp;
jac[0][i] = (f1[0]-f0[0])/h;
jac[1][i] = (f1[1]-f0[1])/h;
jac[2][i] = (f1[2]-f0[2])/h;
}
}
void newton(float weight, float vcg, float tcg, float lcg, float &sinkage, float &heel, float &trim) {
float x[3] = {10,1,1};
float accuracy = 0.000001f;
int ntryes = 30;
int i = 0;
float jac[3][3];
float max;
float f0[3];
float gauss_f0[3];
while (i < ntryes) {
jacobian(x, weight, vcg, tcg, lcg, jac, f0);
if (sqrt((f0[0]*f0[0]+f0[1]*f0[1]+f0[2]*f0[2])/2) < accuracy) {
break;
}
gauss_f0[0] = -f0[0];
gauss_f0[1] = -f0[1];
gauss_f0[2] = -f0[2];
GSolve2(jac, 3, gauss_f0);
x[0] = x[0]+gauss_f0[0];
x[1] = x[1]+gauss_f0[1];
x[2] = x[2]+gauss_f0[2];
// absmax(x) - Return absolute max value from an array
max = absmax(x);
if (max < 1) max = 1;
if (sqrt((gauss_f0[0]*gauss_f0[0]+gauss_f0[1]*gauss_f0[1]+gauss_f0[2]*gauss_f0[2])) < accuracy*max) {
x[0]=x2[0];
x[1]=x2[1];
x[2]=x2[2];
break;
}
i++;
}
sinkage = x[0];
heel = x[1];
trim = x[2];
}
int GSolve2(float a[][3],int n,float b[]) {
float x,sum,max,temp;
int i,j,k,p,m,pos;
int nn = n-1;
for (k=0;k<=n-1;k++)
{
/* pivot*/
max=fabs(a[k][k]);
pos=k;
for (p=k;p<n;p++){
if (max < fabs(a[p][k])){
max=fabs(a[p][k]);
pos=p;
}
}
if (ABS(a[k][pos]) < EPS) {
writeLog("Matrix is singular");
break;
}
if (pos != k) {
for(m=k;m<n;m++){
temp=a[pos][m];
a[pos][m]=a[k][m];
a[k][m]=temp;
}
}
/* convert to upper triangular form */
if ( fabs(a[k][k])>=1.e-6)
{
for (i=k+1;i<n;i++)
{
x = a[i][k]/a[k][k];
for (j=k+1;j<n;j++) a[i][j] = a[i][j] -a[k][j]*x;
b[i] = b[i] - b[k]*x;
}
}
else
{
writeLog("zero pivot found in line:%d",k);
return 0;
}
}
/* back substitution */
b[nn] = b[nn] / a[nn][nn];
for (i=n-2;i>=0;i--)
{
sum = b[i];
for (j=i+1;j<n;j++)
sum = sum - a[i][j]*b[j];
b[i] = sum/a[i][i];
}
return 0;
}
float absmax(float x[]) {
int i = 1;
int n = sizeof(x);
float max = x[0];
while (i < n) {
if (max < x[i]) {
max = x[i];
}
i++;
}
return max;
}
Have you considered some stochastic search methods to find the initial value and then fine-tuning with Newton Raphson? One possibility is evolutionary computation, you can use the Inspyred package. For a physical problem similar in many ways to the one you describe, look at this example: http://inspyred.github.com/tutorial.html#lunar-explorer
What about using a damped version of Newton's method? You could quite easily modify your implementation to make it. Think about Newton's method as finding a direction
d_k = f(x_k) / f'(x_k)
and updating the variable
x_k+1 = x_k - L_k d_k
In the usual Newton's method, L_k is always 1, but this might create overshoots or undershoots. So, let your method chose L_k. Suppose that your method usually overshoots. A possible strategy consists in taking the largest L_k in the set {1,1/2,1/4,1/8,... L_min} such that the condition
|f(x_k+1)| <= (1-L_k/2) |f(x_k)|
is satisfied (or L_min if none of the values satisfies this criteria).
With the same criteria, another possible strategy is to start with L_0=1 and if the criteria is not met, try with L_0/2 until it works (or until L_0 = L_min). Then for L_1, start with min(1, 2L_0) and do the same. Then start with L_2=min(1, 2L_1) and so on.
By the way: are you sure that your problem has a unique solution? I guess that the answer to this question depends on the shape of your object. If you have a rugby ball, there's one angle that you cannot fix. So if your shape is close to such an object, I would not be surprised that the problem is difficult to solve for that angle.