I'm trying to use Drake to solve mixed integer programming problem. One challenge is that my dynamics is nonlinear, with rotation matrix. I tried Gurobi solver to solve this problem, but it shows error like "GurobiSolver is unable to solve because a GenericConstraint was declared but is not supported." May I ask how to deal with this kind of problem in Drake with GurobiSolver?
By the way, I know one way is as this link pointed out, but using SNOPT with hard constraints doesn't produce good results. I think GurobiSolver might be better for this kind of MIQP problem.
When you use rotation matrix in your optimization problem, you will need the SO(3) constraints
RᵀR=I
Do you mean you want to impose this as a quadratic equality constraint? Theoretically Gurobi could handle this non-convex quadratic constraints (but currently not supported from Drake's interface). In practice we find that if you have several non-convex quadratic constraints, Gurobi will be quite slow.
If the only non-convex quadratic constraints in your problem is just rotation matrix SO(3) constraints, then Drake has implemented a customized mixed-integer linear/second-order-cone constraints to approximately satisfy SO(3) constraints. The code is in https://github.com/RobotLocomotion/drake/blob/eb4df7d3cde2db48c697943c43395a3f2b74e00c/solvers/mixed_integer_rotation_constraint.h#L50. One example is in https://github.com/RobotLocomotion/drake/blob/eb4df7d3cde2db48c697943c43395a3f2b74e00c/multibody/inverse_kinematics/global_inverse_kinematics.cc#L113. The math is described in our paper
Related
I am trying to write a program which given some cost and constraints finds a combination of states and torques which will result in a fixed point. The script can be found in this Colab.
For a bit more background, I am trying to balance a humanoid lower body with contact (1 point contact at each foot). So I use Drake's position constraint to get a pose where the feet are on the ground. The contact should be within the friction cone. And the robot should be in equilibrium. My cost function consists of the torques squared and position of the center of mass.
There are a few things going wrong with my optimization:
The SNOPT output tells me that one or multiple (I can only see the constraint with the largest discrepancy) constraints seem to have incorrect gradients. SNOPT also provides the constraint number. Is there a way to see which constraint in drake corresponds to the constraint number given by SNOPT?
Possibly related to 1, SNOPT mostly exits with 2 different error codes: SNOPT 42: Singular basis and SNOPT 43: Cannot satisfy the general constraints. Error code 43 is quite clear, but I don't know how to approach finding a solution to 42.
Lastly, I was wondering what kind of delta is usually acceptable for such an optimization. Currently, of the constraints that fail most have a delta <1E-2 which is still much too large. But maybe my constraints are a bit too strict with the default solver settings?
Is there a way to see which constraint in drake corresponds to the constraint number given by SNOPT?
Unfortunately currently we don't provide the API to see which constraint in drake corresponds to the constraint number given by SNOPT. On the other hand, you could compute the gradient of your Drake constraint numerically, and compare that with the gradient in Eval function. Drake has ComputeNumericalGradient function.
Possibly related to 1, SNOPT mostly exits with 2 different error codes: SNOPT 42: Singular basis and SNOPT 43:
Most likely this is caused by the gradient error.
I would like to know if there is an elegant way to impose acceleration/jerk level constraint on the trajectory optimization using the DirectCollocation class.
I am working with an Acrobot system, and I have already included the velocity level constraint, but I wanted to have a minimum/smooth jerk optimal trajectory.
Thank you in advance.
Yes. The standard direct collocation method (which is implemented in the DirectCollocation class) uses cubic splines to represent the position trajectory. If you take the second derivative, you'll be left with a first-order spline... so the acceleration is always piecewise-linear, and the jerk is always piecewise constant.
So the constraints that you would add would be simple constraints on the spline coefficients of the state trajectory. We don't offer those constraints directly in the API (but could). You could implement them (are you in python or c++) following the pattern here.
It might also help to look at the corresponding section of the course notes if you haven't.
One subtlety -- the current implementation actually represents the state trajectory as the cubic spline (it redundantly represents the positions and velocities). You could opt to add your constraints to either the position trajectory or the velocity trajectory. The constraints should be satisfied perfectly at the knots/collocation points, but the trajectories will be subtly different due to the interpolation.
If I want to optimize a function with respect to some constrained value, I can find a bijective map between an unconstrained space and the constrained space, then optimize the composition of the original function and the bijective map with respect to the unconstrained value.
Does optimizing in a different space affect the performance or accuracy of optimization? And does it vary between bijective maps?
My use case is training constrained Gaussian process model hyperparameters in GPflow using TensorFlow Probability's bijectors.
If I understand you correctly, you might have for example some variable that is constrained to be positive and want to optimize it. And for that you train the variable in the unconstrained space?
That would be pretty common in machine learning where you for example enforce a variance (of let's say the likelihood) to be positive by taking the exponent of the unconstrained value.
I guess the effect on the optimization very much depends on how you optimize it. For gradient based methods it does have an effect, and sometimes small tricks are helpful to improve those issues (e.g. shifting, so that your transformation is tf.exp(shift_val + unconstrained_variable) ).
And yes afaik it varies inbetween different mappings. In my example, the softplus and exponential transformation result in different gradients. Tough I'm not sure if there's a consent on which one is preferable.
I'd just try a few different ones. As long as it doesn't lead to numerical issues, either transformation/bijection should be fine.
EDIT: just to clarify. The bijection should not affect the solution space, just the optimization path itself.
In Svm, this is our dual problem optimization objective with the following constrains along with alphas between 0 and C. How do i find alphas with this optimization objective and these constrians.
Also, please correct me, if i am wrong somewhere
Well, you want to maximize the Lagrangean of this optimization problem, right?
So what you did was to set the partial derivatives of your relevant components to zero to build the Lagrangean. You did that because in the optimum, they are equal to zero. This is the necessary condition to find a stationary point but because of the convexity property of your original problem this is also the optimal solution.
To maximize over the alphas, you want to use the SMO algorithm as described here:
https://en.m.wikipedia.org/wiki/Sequential_minimal_optimization
Good luck!
im trying to write a code that will do projective transformation, but with more than 4 key points. i found this helpful guide but it uses 4 points of reference
https://math.stackexchange.com/questions/296794/finding-the-transform-matrix-from-4-projected-points-with-javascript
i know that matlab uses has a function tcp2form that handles that, but i haven't found a way so far.
anyone can give me some guidance, on how to do so? i can solve the equations using (least squares), but i'm stuck since i have a matrix that is larger than 3*3 and i can't multiple the homogeneous coordinates.
Thanks
If you have more than four control points, you have an overdetermined system of equations. There are two possible scenarios. Either your points are all compatible with the same transformation. In that case, any four points can be used, and the rest will match the transformation exactly. At least in theory. For the sake of numeric stability you'd probably want to choose your points so that they are far from being collinear.
Or your points are not all compatible with a single projective transformation. In this case, all you can hope for is an approximation. If you want the best approximation, you'll have to be more specific about what “best” means, i.e. some kind of error measure. Measuring things in a projective setup is inherently tricky, since there are usually a lot of arbitrary decisions involved.
What you can try is fixing one matrix entry (e.g. the lower right one to 1), then writing the conditions for the remaining 8 coordinates as a system of linear equations, and performing a least squares approximation. But the choice of matrix representative (i.e. fixing one entry here) affects the least squares error measure while it has no effect on the geometric meaning, so this is a pretty arbitrary choice. If the lower right entry of the desired matrix should happen to be zero, you'd computation will run into numeric problems due to overflow.