Is there a standard function that does val = cos(phase) + sin(phas) j. Is there a conventional name for applying "Euler's formula" to do the opposite of the "argument" function? Similar to std::polar, but that expects the magnitude component.
I am looking at a large code base that does the following pattern a lot:
Eigen::VectorXf phase = ...;
Eigen::VectorXcf cplx = ...;
cplx.real() = phase.array().cos();
cplx.imag() = phase.array().sin();
This unfortunately does not take advantage of sincos optimization.
Is there a natural way to do this that I haven't found in the documentation? Is there a conventional name for this?
This is probably what I'm going with, just checking with the world before I change a lot of code.
void phase2Cplx(const Eigen::Ref<const Eigen::VectorXf> phaseIn,
Eigen::Ref<Eigen::VectorXcf> cplxOut) {
// assert len in == len out
Eigen::Index len = cplxOut.size();
std::complex<float> *out = cplxOut.data();
const float *in = phaseIn.data();
for (Eigen::Index i = 0; i < len; ++i) {
float val = in[i];
out[i] = { cosf(val), sinf(val) };
}
}
// #Details: Using () was measurably worse than []
Related
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.
I declared two Eigen::RowVectorXd variables in the program as below. I get wrong results in the compound addition statement sdf_grad+=gradval. Only the first two elements are added and the rest of elements in the sdf_grad vector become 1e19. I don't have any clue why it is happening. Please Help.
Eigen::RowVectorXd sdf_grad(24);
Eigen::VectorXd stress_dof = get_stress_dof();
Eigen::VectorXd strain_dof = get_strain_dof();
for(unsigned int i=0;i!=qn.size(); i++)
{
for(unsigned int j=0; j!=qn.size();j++)
{
double sval = qn[i];
double tval = qn[j];
if(!m_shape->m_set_coordinate)
m_shape->add_coordinates(this->get_xcoords(),this->get_ycoords());
m_shape->update_shapefn(sval,tval);
Eigen::MatrixXd Bs = get_bsmat_local(i,j);
Eigen::Vector3d stress = Bs*stress_dof;
Eigen::MatrixXd Bd = get_bmat(sval,tval);
Eigen::Vector3d strain = Bd* strain_dof;
Eigen::Vector3d cnfn = m_material->get_constitutive_function(stress,strain);
auto WxJ = qw[i] * qw[j] * m_shape->get_detJ();
double delval=cnfn.norm();
objval+=delval*WxJ;
//SETTING GRADIENT OF STRESS DOF
Eigen::MatrixXd CxBs = m_material->get_cmat()*Bs;
Eigen::MatrixXd Bstrans = CxBs.transpose();
Eigen::RowVectorXd gradval= (-WxJ/delval)*Bstrans*cnfn;
sdf_grad+= gradval ; // Wrong Result.
}
}
You did not zero initialize your vector. Write this instead of the first line:
Eigen::RowVectorXd sdf_grad = Eigen::RowVectorXd::Zero(24);
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 am building a program in C++ using the SFML library which plots mathematical functions such as f(x)=sin(x). The code used to plot the points is:
VertexArray curve(PrimitiveType::LineStrip, 100);
for (int x = -50; x < 50; x++)
{
curve.append(Vertex(Vector2f(x,- sin(x))));
}
This code produces this plot:
As you can see the plot is not smooth and is made up of short lines due to sf::Linestrip. Is there a way to ( in SFML ) make this plot smoother ( e.g by shortening the line segments )?
Any feedback is much appreciated : ).
The easiest option is to increase the resolution of the lines - i.e. have more, smaller lines. This is simple to achieve and may be acceptable for your use case.
Sean Cline's example in the comments should be a good starting point:
for (float x = -50.0f; x < 50.0f; x += .25f)
{
curve.append(Vertex(Vector2f(x,- sin(x))));
}
You can then easily generalize the range and step and play around with the values:
float min_range = -200.f;
float max_range = 200.f;
float step = 0.5f;
for (float x = min_range; x < max_range ; x += step)
{
curve.append(Vertex(Vector2f(x,- sin(x))));
}
Finally, you can abstract this away behind a nice interface:
using precision = float;
struct plot_params
{
precision _min_range;
precision _max_range;
precision _step;
};
template <typename TFunction>
auto plot(const plot_params pp, TFunction&& f)
{
assert(pp._min_range <= pp._max_range);
assert(pp._step > 0.f);
VertexArray curve(PrimitiveType::LineStrip,
std::ceil((pp._max_range - pp._min_range) / pp._step);
for (auto x = pp._min_range; x < pp._max_range; x += pp._step)
{
curve.append(Vertex(f(x)));
}
}
And you can use plot as follows:
const auto my_params = []
{
plot_params pp;
pp._min_range = -200.f;
pp._max_range = 200.f;
pp._step = 0.5f;
return pp;
})();
auto curve = plot(my_params,
[](auto x){ return Vector2f(x,- sin(x)); });
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.