void ACSAnt::construct_pseudorandom_proportional_solution(OptimizationProblem &op, PheromoneMatrix &pheromones, double alpha, double beta) {
while(!op.is_tour_complete(tour->get_vertices())) {
std::multimap<double,unsigned int,MultiMapComp> vertices = get_feasible_vertices(op, pheromones, 1.0, beta);
unsigned int q = Util::random_number(RAND_MAX);
double q0 = q0_ * RAND_MAX;
unsigned int vertex;
if (q < q0) {
vertex = (*vertices.begin()).second;
} else {
vertex = choose_next_vertex_with_likelihood(vertices);
}
add_vertex_to_tour(op, vertex);
}
update_tour_length(op);
op.cleanup();
//local pheromone update
for(unsigned int i=0;i<tour->size();i++) {
if(i==0) {
((ACSPheromoneMatrix &) pheromones).local_pheromone_update(pheromones.size()-1, (*tour)[i]);
} else {
((ACSPheromoneMatrix &) pheromones).local_pheromone_update((*tour)[i-1], (*tour)[i]);
}
}
}
SolutionResult MosekSolver::Solve(OptimizationProblem& prog) const {
if (!prog.GetSolverOptionsStr("Mosek").empty()) {
if (prog.GetSolverOptionsStr("Mosek").at("problemtype").find("linear")
!= std::string::npos) {
return MosekLP::Solve(prog);
}
} // TODO(alexdunyak): add more mosek solution types.
return kUnknownError;
}
void Ant::construct_rational_solution(OptimizationProblem &op, PheromoneMatrix &pheromones, double alpha, double beta) {
while(!op.is_tour_complete(tour->get_vertices())) {
std::multimap<double,unsigned int,MultiMapComp> vertices = get_feasible_vertices(op, pheromones, alpha, beta);
unsigned int vertex = (*vertices.begin()).second;
add_vertex_to_tour(op, vertex);
}
update_tour_length(op);
op.cleanup();
}
void Ant::construct_random_proportional_solution(OptimizationProblem &op, PheromoneMatrix &pheromones, double alpha, double beta) {
while(!op.is_tour_complete(tour->get_vertices())) {
std::multimap<double,unsigned int,MultiMapComp> vertices = get_feasible_vertices(op, pheromones, alpha, beta);
unsigned int vertex = choose_next_vertex_with_likelihood(vertices);
add_vertex_to_tour(op, vertex);
}
update_tour_length(op);
op.cleanup();
}
std::multimap<double,unsigned int,Ant::MultiMapComp> Ant::get_feasible_vertices(OptimizationProblem &op, PheromoneMatrix &pheromones, double alpha, double beta) {
std::map<unsigned int,double> vertices;
bool should_debug = false;
if(current_vertex() == -1) {
vertices = op.get_feasible_start_vertices();
} else {
vertices = op.get_feasible_neighbours(current_vertex());
}
double denominator = 0.0;
std::map<unsigned int,double>::iterator it;
for(it=vertices.begin();it!=vertices.end();it++) {
unsigned int vertex = (*it).first;
double heuristic_value = (*it).second;
if(current_vertex() == -1) {
double numerator = pow(pheromones.get(pheromones.size()-1, vertex), alpha) * pow(heuristic_value, beta);
(*it).second = numerator;
denominator += numerator;
} else {
double numerator = pow(pheromones.get(current_vertex(), vertex), alpha) * pow(heuristic_value, beta);
(*it).second = numerator;
denominator += numerator;
if (should_debug) {
std::cerr << "\tEdge to " << vertex << std::endl;
std::cerr << "\tpow(" << pheromones.get(current_vertex(), vertex) << "," << alpha << ") * pow(" << heuristic_value << "," << beta << ")" << std::endl;
std::cerr << "\tNumertor is " << numerator << " and current denom = " << denominator << std::endl;
}
}
}
std::multimap<double,unsigned int,MultiMapComp> probabilities;
for(it=vertices.begin();it!=vertices.end();it++) {
//std::cout << pheromones.get(pheromones.size()-1, (*it).first) << std::endl;
unsigned int vertex = (*it).first;
//double heuristic_value = (*it).second;
double probability;
if(denominator == 0) {
probability = 1.0 / vertices.size();
} else {
probability = (*it).second / denominator;
}
if (should_debug)
std::cerr << "Edge to " << vertex << " has probability " << probability << std::endl;
//std::cout << pheromones.get(current_vertex(), vertex) << " " << heuristic_value << std::endl;
//std::cout << "vertex: " << vertex << " heuristic: " << heuristic_value << " probability: " << probability << std::endl;
probabilities.insert(std::pair<double,unsigned int>(probability, vertex));
}
//std::cout << "***************************************" << std::endl;
return probabilities;
}
double compute_average_pheromone_update(OptimizationProblem &op) {
SimpleAnt ant(op.get_max_tour_size());
PheromoneMatrix matrix(op.number_of_vertices(), 0.0, 1.0);
ant.construct_rational_solution(op, matrix, 0, 1);
std::vector<unsigned int> tour = ant.get_vertices();
double tour_length = op.eval_tour(tour);
double pheromone_sum = 0.0;
for(unsigned int i=0;i<tour.size();i++) {
pheromone_sum += op.pheromone_update(tour[i], tour_length);
}
double pheromone_avg = pheromone_sum / tour.size();
return pheromone_avg;
}
int GetIKSolverInfo(const OptimizationProblem& prog, SolutionResult result) {
std::string solver_name;
int solver_result = 0;
prog.GetSolverResult(&solver_name, &solver_result);
if (solver_name == "SNOPT") {
// We can return SNOPT results directly.
return solver_result;
}
// Make a SNOPT-like return code out of the generic result.
switch (result) {
case SolutionResult::kSolutionFound: {
return 1;
}
case SolutionResult::kInvalidInput: {
return 91;
}
case SolutionResult::kInfeasibleConstraints: {
return 13;
}
case SolutionResult::kUnknownError: {
return 100; // Not a real SNOPT error.
}
}
return -1;
}
void ACSAnt::offline_pheromone_update(OptimizationProblem &op, PheromoneMatrix &pheromones, double weight) {
for(unsigned int i=0;i<tour->size();i++) {
double pheromone = op.pheromone_update((*tour)[i], tour->get_length());
if(i==0) {
pheromones.add(pheromones.size()-1, (*tour)[i], weight * pheromones.get_evaporation_rate() * pheromone);
} else {
pheromones.add((*tour)[i-1], (*tour)[i], weight * pheromones.get_evaporation_rate() * pheromone);
}
}
}
DRAKERBSYSTEM_EXPORT RigidBodySystem::StateVector<double>
Drake::getInitialState(const RigidBodySystem& sys) {
VectorXd x0(sys.tree->num_positions + sys.tree->num_velocities);
default_random_engine generator;
x0 << sys.tree->getRandomConfiguration(generator),
VectorXd::Random(sys.tree->num_velocities);
// todo: implement joint limits, etc.
if (sys.tree->getNumPositionConstraints()) {
// todo: move this up to the system level?
OptimizationProblem prog;
std::vector<RigidBodyLoop, Eigen::aligned_allocator<RigidBodyLoop> > const&
loops = sys.tree->loops;
int nq = sys.tree->num_positions;
auto qvar = prog.AddContinuousVariables(nq);
Matrix<double, 7, 1> bTbp = Matrix<double, 7, 1>::Zero();
bTbp(3) = 1.0;
Vector2d tspan;
tspan << -std::numeric_limits<double>::infinity(),
std::numeric_limits<double>::infinity();
Vector3d zero = Vector3d::Zero();
for (int i = 0; i < loops.size(); i++) {
auto con1 = make_shared<RelativePositionConstraint>(
sys.tree.get(), zero, zero, zero, loops[i].frameA->frame_index,
loops[i].frameB->frame_index, bTbp, tspan);
std::shared_ptr<SingleTimeKinematicConstraintWrapper> con1wrapper(
new SingleTimeKinematicConstraintWrapper(con1));
prog.AddGenericConstraint(con1wrapper, {qvar});
auto con2 = make_shared<RelativePositionConstraint>(
sys.tree.get(), loops[i].axis, loops[i].axis, loops[i].axis,
loops[i].frameA->frame_index, loops[i].frameB->frame_index, bTbp,
tspan);
std::shared_ptr<SingleTimeKinematicConstraintWrapper> con2wrapper(
new SingleTimeKinematicConstraintWrapper(con2));
prog.AddGenericConstraint(con2wrapper, {qvar});
}
VectorXd q_guess = x0.topRows(nq);
prog.AddQuadraticCost(MatrixXd::Identity(nq, nq), q_guess);
prog.Solve();
x0 << qvar.value(), VectorXd::Zero(sys.tree->num_velocities);
}
return x0;
}
RigidBodySystem::StateVector<double> RigidBodySystem::dynamics(const double& t, const RigidBodySystem::StateVector<double>& x, const RigidBodySystem::InputVector<double>& u) const {
using namespace std;
using namespace Eigen;
eigen_aligned_unordered_map<const RigidBody *, Matrix<double, 6, 1> > f_ext;
// todo: make kinematics cache once and re-use it (but have to make one per type)
auto nq = tree->num_positions;
auto nv = tree->num_velocities;
auto num_actuators = tree->actuators.size();
auto q = x.topRows(nq);
auto v = x.bottomRows(nv);
auto kinsol = tree->doKinematics(q,v);
// todo: preallocate the optimization problem and constraints, and simply update them then solve on each function eval.
// happily, this clunkier version seems fast enough for now
// the optimization framework should support this (though it has not been tested thoroughly yet)
OptimizationProblem prog;
auto const & vdot = prog.addContinuousVariables(nv,"vdot");
auto H = tree->massMatrix(kinsol);
Eigen::MatrixXd H_and_neg_JT = H;
{ // loop through rigid body force elements and accumulate f_ext
// todo: distinguish between AppliedForce and ConstraintForce
// todo: have AppliedForce output tau (instead of f_ext). it's more direct, and just as easy to compute.
int u_index = 0;
const NullVector<double> force_state; // todo: will have to handle this case
for (auto const & prop : props) {
RigidBodyFrame* frame = prop->getFrame();
RigidBody* body = frame->body.get();
int num_inputs = 1; // todo: generalize this
RigidBodyPropellor::InputVector<double> u_i(u.middleRows(u_index,num_inputs));
// todo: push the frame to body transform into the dynamicsBias method?
Matrix<double,6,1> f_ext_i = transformSpatialForce(frame->transform_to_body,prop->output(t,force_state,u_i,kinsol));
if (f_ext.find(body)==f_ext.end()) f_ext[body] = f_ext_i;
else f_ext[body] = f_ext[body]+f_ext_i;
u_index += num_inputs;
}
}
VectorXd C = tree->dynamicsBiasTerm(kinsol,f_ext);
if (num_actuators > 0) C -= tree->B*u.topRows(num_actuators);
{ // apply contact forces
const bool use_multi_contact = false;
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
if (use_multi_contact)
tree->potentialCollisions(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx);
else
tree->collisionDetect(kinsol,phi,normal,xA,xB,bodyA_idx,bodyB_idx);
for (int i=0; i<phi.rows(); i++) {
if (phi(i)<0.0) { // then i have contact
double mu = 1.0; // todo: make this a parameter
// todo: move this entire block to a shared an updated "contactJacobian" method in RigidBodyTree
auto JA = tree->transformPointsJacobian(kinsol, xA.col(i), bodyA_idx[i], 0, false);
auto JB = tree->transformPointsJacobian(kinsol, xB.col(i), bodyB_idx[i], 0, false);
Vector3d this_normal = normal.col(i);
// compute the surface tangent basis
Vector3d tangent1;
if (1.0 - this_normal(2) < EPSILON) { // handle the unit-normal case (since it's unit length, just check z)
tangent1 << 1.0, 0.0, 0.0;
} else if (1 + this_normal(2) < EPSILON) {
tangent1 << -1.0, 0.0, 0.0; //same for the reflected case
} else {// now the general case
tangent1 << this_normal(1), -this_normal(0), 0.0;
tangent1 /= sqrt(this_normal(1)*this_normal(1) + this_normal(0)*this_normal(0));
}
Vector3d tangent2 = tangent1.cross(this_normal);
Matrix3d R; // rotation into normal coordinates
R << tangent1, tangent2, this_normal;
auto J = R*(JA-JB); // J = [ D1; D2; n ]
auto relative_velocity = J*v; // [ tangent1dot; tangent2dot; phidot ]
if (false) {
// spring law for normal force: fA_normal = -k*phi - b*phidot
// and damping for tangential force: fA_tangent = -b*tangentdot (bounded by the friction cone)
double k = 150, b = k / 10; // todo: put these somewhere better... or make them parameters?
Vector3d fA;
fA(2) = -k * phi(i) - b * relative_velocity(2);
fA.head(2) = -std::min(b, mu*fA(2)/(relative_velocity.head(2).norm()+EPSILON)) * relative_velocity.head(2); // epsilon to avoid divide by zero
// equal and opposite: fB = -fA.
// tau = (R*JA)^T fA + (R*JB)^T fB = J^T fA
C -= J.transpose()*fA;
} else { // linear acceleration constraints (more expensive, but less tuning required for robot mass, etc)
// note: this does not work for the multi-contact case (it overly constrains the motion of the link). Perhaps if I made them inequality constraints...
static_assert(!use_multi_contact, "The acceleration contact constraints do not play well with multi-contact");
// phiddot = -2*alpha*phidot - alpha^2*phi // critically damped response
// tangential_velocity_dot = -2*alpha*tangential_velocity
double alpha = 20; // todo: put this somewhere better... or make them parameters?
Vector3d desired_relative_acceleration = -2*alpha*relative_velocity;
desired_relative_acceleration(2) += -alpha*alpha*phi(i);
// relative_acceleration = J*vdot + R*(JAdotv - JBdotv) // uses the standard dnormal/dq = 0 assumption
cout << "phi = " << phi << endl;
cout << "desired acceleration = " << desired_relative_acceleration.transpose() << endl;
// cout << "acceleration = " << (J*vdot + R*(JAdotv - JBdotv)).transpose() << endl;
prog.addContinuousVariables(3,"contact normal force");
auto JAdotv = tree->transformPointsJacobianDotTimesV(kinsol, xA.col(i).eval(), bodyA_idx[i], 0);
auto JBdotv = tree->transformPointsJacobianDotTimesV(kinsol, xB.col(i).eval(), bodyB_idx[i], 0);
prog.addLinearEqualityConstraint(J,desired_relative_acceleration - R*(JAdotv - JBdotv),{vdot});
H_and_neg_JT.conservativeResize(NoChange,H_and_neg_JT.cols()+3);
H_and_neg_JT.rightCols(3) = -J.transpose();
}
}
}
}
if (tree->getNumPositionConstraints()) {
int nc = tree->getNumPositionConstraints();
const double alpha = 5.0; // 1/time constant of position constraint satisfaction (see my latex rigid body notes)
prog.addContinuousVariables(nc,"position constraint force"); // don't actually need to use the decision variable reference that would be returned...
// then compute the constraint force
auto phi = tree->positionConstraints(kinsol);
auto J = tree->positionConstraintsJacobian(kinsol,false);
auto Jdotv = tree->positionConstraintsJacDotTimesV(kinsol);
// phiddot = -2 alpha phidot - alpha^2 phi (0 + critically damped stabilization term)
prog.addLinearEqualityConstraint(J,-(Jdotv + 2*alpha*J*v + alpha*alpha*phi),{vdot});
H_and_neg_JT.conservativeResize(NoChange,H_and_neg_JT.cols()+J.rows());
H_and_neg_JT.rightCols(J.rows()) = -J.transpose();
}
// add [H,-J^T]*[vdot;f] = -C
prog.addLinearEqualityConstraint(H_and_neg_JT,-C);
prog.solve();
// prog.printSolution();
StateVector<double> dot(nq+nv);
dot << kinsol.transformPositionDotMappingToVelocityMapping(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(nq, nq))*v, vdot.value();
return dot;
}
SolutionResult Drake::SnoptSolver::Solve(
OptimizationProblem& prog) const {
auto d = prog.GetSolverData<SNOPTData>();
SNOPTRun cur(*d);
Drake::OptimizationProblem const* current_problem = &prog;
// Set the "maxcu" value to tell snopt to reserve one 8-char entry of user
// workspace. We are then allowed to use cw(snopt_mincw+1:maxcu), as
// expressed in Fortran array slicing. Use the space to pass our problem
// instance pointer to our userfun.
cur.snSeti("User character workspace", snopt_mincw + 1);
{
char const* const pcp = reinterpret_cast<char*>(¤t_problem);
char* const cu_cp = d->cw.data() + 8 * snopt_mincw;
std::copy(pcp, pcp + sizeof(current_problem), cu_cp);
}
snopt::integer nx = prog.num_vars();
d->min_alloc_x(nx);
snopt::doublereal* x = d->x.data();
snopt::doublereal* xlow = d->xlow.data();
snopt::doublereal* xupp = d->xupp.data();
const Eigen::VectorXd x_initial_guess = prog.initial_guess();
for (int i = 0; i < nx; i++) {
x[i] = static_cast<snopt::doublereal>(x_initial_guess(i));
xlow[i] =
static_cast<snopt::doublereal>(-std::numeric_limits<double>::infinity());
xupp[i] =
static_cast<snopt::doublereal>(std::numeric_limits<double>::infinity());
}
for (auto const& binding : prog.bounding_box_constraints()) {
auto const& c = binding.constraint();
for (const DecisionVariableView& v : binding.variable_list()) {
auto const lb = c->lower_bound(), ub = c->upper_bound();
for (int k = 0; k < v.size(); k++) {
xlow[v.index() + k] = std::max<snopt::doublereal>(
static_cast<snopt::doublereal>(lb(k)), xlow[v.index() + k]);
xupp[v.index() + k] = std::min<snopt::doublereal>(
static_cast<snopt::doublereal>(ub(k)), xupp[v.index() + k]);
}
}
}
size_t num_nonlinear_constraints = 0, max_num_gradients = nx;
for (auto const& binding : prog.generic_constraints()) {
auto const& c = binding.constraint();
size_t n = c->num_constraints();
for (const DecisionVariableView& v : binding.variable_list()) {
max_num_gradients += n * v.size();
}
num_nonlinear_constraints += n;
}
size_t num_linear_constraints = 0;
for (auto const& binding : prog.linear_equality_constraints()) {
num_linear_constraints += binding.constraint()->num_constraints();
}
snopt::integer nF = 1 + num_nonlinear_constraints + num_linear_constraints;
d->min_alloc_F(nF);
snopt::doublereal* Flow = d->Flow.data();
snopt::doublereal* Fupp = d->Fupp.data();
Flow[0] =
static_cast<snopt::doublereal>(-std::numeric_limits<double>::infinity());
Fupp[0] =
static_cast<snopt::doublereal>(std::numeric_limits<double>::infinity());
snopt::integer lenG = static_cast<snopt::integer>(max_num_gradients);
d->min_alloc_G(lenG);
snopt::integer* iGfun = d->iGfun.data();
snopt::integer* jGvar = d->jGvar.data();
for (snopt::integer i = 0; i < nx; i++) {
iGfun[i] = 1;
jGvar[i] = i + 1;
}
size_t constraint_index = 1, grad_index = nx; // constraint index starts at 1
// because the objective is the
// first row
for (auto const& binding : prog.generic_constraints()) {
auto const& c = binding.constraint();
size_t n = c->num_constraints();
auto const lb = c->lower_bound(), ub = c->upper_bound();
for (int i = 0; i < n; i++) {
Flow[constraint_index + i] = static_cast<snopt::doublereal>(lb(i));
Fupp[constraint_index + i] = static_cast<snopt::doublereal>(ub(i));
}
for (const DecisionVariableView& v : binding.variable_list()) {
for (size_t i = 0; i < n; i++) {
for (size_t j = 0; j < v.size(); j++) {
iGfun[grad_index] = constraint_index + i + 1; // row order
jGvar[grad_index] = v.index() + j + 1;
grad_index++;
}
}
}
constraint_index += n;
}
// http://eigen.tuxfamily.org/dox/group__TutorialSparse.html
typedef Eigen::Triplet<double> T;
std::vector<T> tripletList;
tripletList.reserve(num_linear_constraints * prog.num_vars());
size_t linear_constraint_index = 0;
for (auto const& binding : prog.linear_equality_constraints()) {
auto const& c = binding.constraint();
size_t n = c->num_constraints();
size_t var_index = 0;
Eigen::SparseMatrix<double> A_constraint = c->GetSparseMatrix();
for (const DecisionVariableView& v : binding.variable_list()) {
for (size_t k = 0; k < v.size(); ++k) {
for (Eigen::SparseMatrix<double>::InnerIterator it(
A_constraint, var_index + k);
it; ++it) {
tripletList.push_back(
T(linear_constraint_index + it.row(), v.index() + k, it.value()));
}
}
var_index += v.size();
}
auto const lb = c->lower_bound(), ub = c->upper_bound();
for (int i = 0; i < n; i++) {
Flow[constraint_index + i] = static_cast<snopt::doublereal>(lb(i));
Fupp[constraint_index + i] = static_cast<snopt::doublereal>(ub(i));
}
constraint_index += n;
linear_constraint_index += n;
}
snopt::integer lenA = static_cast<snopt::integer>(tripletList.size());
d->min_alloc_A(lenA);
snopt::doublereal* A = d->A.data();
snopt::integer* iAfun = d->iAfun.data();
snopt::integer* jAvar = d->jAvar.data();
size_t A_index = 0;
for (auto const& it : tripletList) {
A[A_index] = it.value();
iAfun[A_index] = 1 + num_nonlinear_constraints + it.row() + 1;
jAvar[A_index] = it.col() + 1;
A_index++;
}
snopt::integer nxname = 1, nFname = 1, npname = 0;
char xnames[8 * 1]; // should match nxname
char Fnames[8 * 1]; // should match nFname
char Prob[200] = "drake.out";
snopt::integer nS, nInf;
snopt::doublereal sInf;
if (true) { // print to output file (todo: make this an option)
cur.iPrint = 9;
char print_file_name[50] = "snopt.out";
snopt::integer print_file_name_len =
static_cast<snopt::integer>(strlen(print_file_name));
snopt::integer inform;
snopt::snopenappend_(&cur.iPrint, print_file_name, &inform,
print_file_name_len);
cur.snSeti("Major print level", static_cast<snopt::integer>(11));
cur.snSeti("Print file", cur.iPrint);
}
snopt::integer minrw, miniw, mincw;
cur.snMemA(nF, nx, nxname, nFname, lenA, lenG, &mincw, &miniw, &minrw);
d->min_alloc_w(mincw, miniw, minrw);
cur.snSeti("Total character workspace", d->lencw);
cur.snSeti("Total integer workspace", d->leniw);
cur.snSeti("Total real workspace", d->lenrw);
snopt::integer Cold = 0;
snopt::doublereal* xmul = d->xmul.data();
snopt::integer* xstate = d->xstate.data();
memset(xstate, 0, sizeof(snopt::integer) * nx);
snopt::doublereal* F = d->F.data();
snopt::doublereal* Fmul = d->Fmul.data();
snopt::integer* Fstate = d->Fstate.data();
memset(Fstate, 0, sizeof(snopt::integer) * nF);
snopt::doublereal ObjAdd = 0.0;
snopt::integer ObjRow = 1; // feasibility problem (for now)
// TODO(sam.creasey) These should be made into options when #1879 is
// resolved or deleted.
/*
mysnseti("Derivative
option",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"DerivativeOption"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Major iterations
limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"MajorIterationsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Minor iterations
limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"MinorIterationsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnsetr("Major optimality
tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"MajorOptimalityTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnsetr("Major feasibility
tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"MajorFeasibilityTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnsetr("Minor feasibility
tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"MinorFeasibilityTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Superbasics
limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"SuperbasicsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Verify
level",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"VerifyLevel"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Iterations
Limit",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"IterationsLimit"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Scale
option",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"ScaleOption"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("New basis
file",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"NewBasisFile"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Old basis
file",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"OldBasisFile"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnseti("Backup basis
file",static_cast<snopt::integer>(*mxGetPr(mxGetField(prhs[13],0,"BackupBasisFile"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
mysnsetr("Linesearch
tolerance",static_cast<snopt::doublereal>(*mxGetPr(mxGetField(prhs[13],0,"LinesearchTolerance"))),&iPrint,&iSumm,&INFO_snopt,cw.get(),&lencw,iw.get(),&leniw,rw.get(),&lenrw);
*/
snopt::integer info;
snopt::snopta_(
&Cold, &nF, &nx, &nxname, &nFname, &ObjAdd, &ObjRow, Prob, snopt_userfun,
iAfun, jAvar, &lenA, &lenA, A, iGfun, jGvar, &lenG, &lenG, xlow, xupp,
xnames, Flow, Fupp, Fnames, x, xstate, xmul, F, Fstate, Fmul, &info,
&mincw, &miniw, &minrw, &nS, &nInf, &sInf, d->cw.data(), &d->lencw,
d->iw.data(), &d->leniw, d->rw.data(), &d->lenrw,
// Pass the snopt workspace as the user workspace. We already set
// the maxcu option to reserve some of it for our user code.
d->cw.data(), &d->lencw, d->iw.data(), &d->leniw, d->rw.data(), &d->lenrw,
npname, 8 * nxname, 8 * nFname, 8 * d->lencw, 8 * d->lencw);
Eigen::VectorXd sol(nx);
for (int i = 0; i < nx; i++) {
sol(i) = static_cast<double>(x[i]);
}
prog.SetDecisionVariableValues(sol);
prog.SetSolverResult("SNOPT", info);
// todo: extract the other useful quantities, too.
if (info >= 1 && info <= 6) {
return SolutionResult::kSolutionFound;
} else if (info >= 11 && info <= 16) {
return SolutionResult::kInfeasibleConstraints;
} else if (info == 91) {
return SolutionResult::kInvalidInput;
}
return SolutionResult::kUnknownError;
}
RigidBodySystem::StateVector<double> RigidBodySystem::dynamics(
const double& t, const RigidBodySystem::StateVector<double>& x,
const RigidBodySystem::InputVector<double>& u) const {
using namespace std;
using namespace Eigen;
eigen_aligned_unordered_map<const RigidBody*, Matrix<double, 6, 1> > f_ext;
// todo: make kinematics cache once and re-use it (but have to make one per
// type)
auto nq = tree->num_positions;
auto nv = tree->num_velocities;
auto num_actuators = tree->actuators.size();
auto q = x.topRows(nq);
auto v = x.bottomRows(nv);
auto kinsol = tree->doKinematics(q, v);
// todo: preallocate the optimization problem and constraints, and simply
// update them then solve on each function eval.
// happily, this clunkier version seems fast enough for now
// the optimization framework should support this (though it has not been
// tested thoroughly yet)
OptimizationProblem prog;
auto const& vdot = prog.AddContinuousVariables(nv, "vdot");
auto H = tree->massMatrix(kinsol);
Eigen::MatrixXd H_and_neg_JT = H;
VectorXd C = tree->dynamicsBiasTerm(kinsol, f_ext);
if (num_actuators > 0) C -= tree->B * u.topRows(num_actuators);
// loop through rigid body force elements
{
// todo: distinguish between AppliedForce and ConstraintForce
size_t u_index = 0;
for (auto const& f : force_elements) {
size_t num_inputs = f->getNumInputs();
VectorXd force_input(u.middleRows(u_index, num_inputs));
C -= f->output(t, force_input, kinsol);
u_index += num_inputs;
}
}
// apply joint limit forces
{
for (auto const& b : tree->bodies) {
if (!b->hasParent()) continue;
auto const& joint = b->getJoint();
if (joint.getNumPositions() == 1 &&
joint.getNumVelocities() ==
1) { // taking advantage of only single-axis joints having joint
// limits makes things easier/faster here
double qmin = joint.getJointLimitMin()(0),
qmax = joint.getJointLimitMax()(0);
// tau = k*(qlimit-q) - b(qdot)
if (q(b->position_num_start) < qmin)
C(b->velocity_num_start) -=
penetration_stiffness * (qmin - q(b->position_num_start)) -
penetration_damping * v(b->velocity_num_start);
else if (q(b->position_num_start) > qmax)
C(b->velocity_num_start) -=
penetration_stiffness * (qmax - q(b->position_num_start)) -
penetration_damping * v(b->velocity_num_start);
}
}
}
// apply contact forces
{
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
if (use_multi_contact)
tree->potentialCollisions(kinsol, phi, normal, xA, xB, bodyA_idx,
bodyB_idx);
else
tree->collisionDetect(kinsol, phi, normal, xA, xB, bodyA_idx, bodyB_idx);
for (int i = 0; i < phi.rows(); i++) {
if (phi(i) < 0.0) { // then i have contact
// todo: move this entire block to a shared an updated "contactJacobian"
// method in RigidBodyTree
auto JA = tree->transformPointsJacobian(kinsol, xA.col(i), bodyA_idx[i],
0, false);
auto JB = tree->transformPointsJacobian(kinsol, xB.col(i), bodyB_idx[i],
0, false);
Vector3d this_normal = normal.col(i);
// compute the surface tangent basis
Vector3d tangent1;
if (1.0 - this_normal(2) < EPSILON) { // handle the unit-normal case
// (since it's unit length, just
// check z)
tangent1 << 1.0, 0.0, 0.0;
} else if (1 + this_normal(2) < EPSILON) {
tangent1 << -1.0, 0.0, 0.0; // same for the reflected case
} else { // now the general case
tangent1 << this_normal(1), -this_normal(0), 0.0;
tangent1 /= sqrt(this_normal(1) * this_normal(1) +
this_normal(0) * this_normal(0));
}
Vector3d tangent2 = this_normal.cross(tangent1);
Matrix3d R; // rotation into normal coordinates
R.row(0) = tangent1;
R.row(1) = tangent2;
R.row(2) = this_normal;
auto J = R * (JA - JB); // J = [ D1; D2; n ]
auto relative_velocity = J * v; // [ tangent1dot; tangent2dot; phidot ]
{
// spring law for normal force: fA_normal = -k*phi - b*phidot
// and damping for tangential force: fA_tangent = -b*tangentdot
// (bounded by the friction cone)
Vector3d fA;
fA(2) =
std::max<double>(-penetration_stiffness * phi(i) -
penetration_damping * relative_velocity(2),
0.0);
fA.head(2) =
-std::min<double>(
penetration_damping,
friction_coefficient * fA(2) /
(relative_velocity.head(2).norm() + EPSILON)) *
relative_velocity.head(2); // epsilon to avoid divide by zero
// equal and opposite: fB = -fA.
// tau = (R*JA)^T fA + (R*JB)^T fB = J^T fA
C -= J.transpose() * fA;
}
}
}
}
if (tree->getNumPositionConstraints()) {
int nc = tree->getNumPositionConstraints();
const double alpha = 5.0; // 1/time constant of position constraint
// satisfaction (see my latex rigid body notes)
prog.AddContinuousVariables(
nc, "position constraint force"); // don't actually need to use the
// decision variable reference that
// would be returned...
// then compute the constraint force
auto phi = tree->positionConstraints(kinsol);
auto J = tree->positionConstraintsJacobian(kinsol, false);
auto Jdotv = tree->positionConstraintsJacDotTimesV(kinsol);
// phiddot = -2 alpha phidot - alpha^2 phi (0 + critically damped
// stabilization term)
prog.AddLinearEqualityConstraint(
J, -(Jdotv + 2 * alpha * J * v + alpha * alpha * phi), {vdot});
H_and_neg_JT.conservativeResize(NoChange, H_and_neg_JT.cols() + J.rows());
H_and_neg_JT.rightCols(J.rows()) = -J.transpose();
}
// add [H,-J^T]*[vdot;f] = -C
prog.AddLinearEqualityConstraint(H_and_neg_JT, -C);
prog.Solve();
// prog.PrintSolution();
StateVector<double> dot(nq + nv);
dot << kinsol.transformPositionDotMappingToVelocityMapping(
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Identity(
nq, nq)) *
v,
vdot.value();
return dot;
}
void Ant::apply_local_search(OptimizationProblem &op) {
std::vector<unsigned int> vertices = op.apply_local_search(tour->get_vertices());
tour->set_vertices(vertices);
update_tour_length(op);
}
void Ant::update_tour_length(OptimizationProblem &op) {
tour->set_length(op.eval_tour(tour->get_vertices()));
}
void Ant::add_vertex_to_tour(OptimizationProblem &op, unsigned int vertex) {
tour->add_vertex(vertex);
op.added_vertex_to_tour(vertex);
}
void inverseKinBackend(
RigidBodyTree* model, const int nT,
const double* t, const MatrixBase<DerivedA>& q_seed,
const MatrixBase<DerivedB>& q_nom, const int num_constraints,
RigidBodyConstraint** const constraint_array,
const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
int* info, std::vector<std::string>* infeasible_constraint) {
// Validate some basic parameters of the input.
if (q_seed.rows() != model->number_of_positions() || q_seed.cols() != nT ||
q_nom.rows() != model->number_of_positions() || q_nom.cols() != nT) {
throw std::runtime_error(
"Drake::inverseKinBackend: q_seed and q_nom must be of size "
"nq x nT");
}
KinematicsCacheHelper<double> kin_helper(model->bodies);
// TODO(sam.creasey) I really don't like rebuilding the
// OptimizationProblem for every timestep, but it's not possible to
// enable/disable (or even remove) a constraint from an
// OptimizationProblem between calls to Solve() currently, so
// there's not actually another way.
for (int t_index = 0; t_index < nT; t_index++) {
OptimizationProblem prog;
SetIKSolverOptions(ikoptions, &prog);
DecisionVariableView vars =
prog.AddContinuousVariables(model->number_of_positions());
MatrixXd Q;
ikoptions.getQ(Q);
auto objective = std::make_shared<InverseKinObjective>(model, Q);
prog.AddCost(objective, {vars});
for (int i = 0; i < num_constraints; i++) {
RigidBodyConstraint* constraint = constraint_array[i];
const int constraint_category = constraint->getCategory();
if (constraint_category ==
RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
SingleTimeKinematicConstraint* stc =
static_cast<SingleTimeKinematicConstraint*>(constraint);
if (!stc->isTimeValid(&t[t_index])) { continue; }
auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>(
stc, &kin_helper);
prog.AddConstraint(wrapper, {vars});
} else if (constraint_category ==
RigidBodyConstraint::PostureConstraintCategory) {
PostureConstraint* pc = static_cast<PostureConstraint*>(constraint);
if (!pc->isTimeValid(&t[t_index])) { continue; }
VectorXd lb;
VectorXd ub;
pc->bounds(&t[t_index], lb, ub);
prog.AddBoundingBoxConstraint(lb, ub, {vars});
} else if (
constraint_category ==
RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) {
AddSingleTimeLinearPostureConstraint(
&t[t_index], constraint, model->number_of_positions(),
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::QuasiStaticConstraintCategory) {
AddQuasiStaticConstraint(&t[t_index], constraint, &kin_helper,
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) {
throw std::runtime_error(
"MultipleTimeKinematicConstraint is not supported"
" in pointwise mode.");
} else if (
constraint_category ==
RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) {
throw std::runtime_error(
"MultipleTimeLinearPostureConstraint is not supported"
" in pointwise mode.");
}
}
// Add a bounding box constraint from the model.
prog.AddBoundingBoxConstraint(
model->joint_limit_min, model->joint_limit_max,
{vars});
// TODO(sam.creasey) would this be faster if we stored the view
// instead of copying into a VectorXd?
objective->set_q_nom(q_nom.col(t_index));
if (!ikoptions.getSequentialSeedFlag() || (t_index == 0)) {
prog.SetInitialGuess(vars, q_seed.col(t_index));
} else {
prog.SetInitialGuess(vars, q_sol->col(t_index - 1));
}
SolutionResult result = prog.Solve();
prog.PrintSolution();
q_sol->col(t_index) = vars.value();
info[t_index] = GetIKSolverInfo(prog, result);
}
}
