//==============================================================================
void QueuedTrajectoryExecutor::cancel()
{
std::lock_guard<std::mutex> lock(mMutex);
DART_UNUSED(lock); // Suppress unused variable warning
std::exception_ptr cancel
= std::make_exception_ptr(std::runtime_error("Trajectory canceled."));
if (mInProgress)
{
mExecutor->cancel();
// Set our own exception, since cancel may not be supported
auto promise = mPromiseQueue.front();
mPromiseQueue.pop();
promise->set_exception(cancel);
mInProgress = false;
}
// Trajectory and promise queue are now the same length
while (!mPromiseQueue.empty())
{
auto promise = mPromiseQueue.front();
mPromiseQueue.pop();
promise->set_exception(cancel);
mTrajectoryQueue.pop();
}
mFuture = std::future<void>();
}
//==============================================================================
std::future<void> QueuedTrajectoryExecutor::execute(
const trajectory::ConstTrajectoryPtr& traj)
{
validate(traj.get());
{
std::lock_guard<std::mutex> lock(mMutex);
DART_UNUSED(lock); // Suppress unused variable warning
// Queue the trajectory and promise that will need to be set
mTrajectoryQueue.push(std::move(traj));
mPromiseQueue.emplace(new std::promise<void>());
return mPromiseQueue.back()->get_future();
}
}
//==============================================================================
void QueuedTrajectoryExecutor::step(
const std::chrono::system_clock::time_point& timepoint)
{
mExecutor->step(timepoint);
std::lock_guard<std::mutex> lock(mMutex);
DART_UNUSED(lock); // Suppress unused variable warning
// If a trajectory was executing, check if it has finished
if (mInProgress)
{
// Return if the trajectory is still executing
if (mFuture.wait_for(std::chrono::seconds(0)) != std::future_status::ready)
return;
mInProgress = false;
// The promise corresponding to the trajectory that just finished must be
// at the front of its queue.
auto promise = mPromiseQueue.front();
mPromiseQueue.pop();
// Propagate the future's value or exception to the caller
try
{
mFuture.get();
promise->set_value();
}
catch (const std::exception& e)
{
promise->set_exception(std::current_exception());
cancel();
}
}
// No trajectory currently executing, execute a trajectory from the queue
if (!mTrajectoryQueue.empty())
{
trajectory::ConstTrajectoryPtr traj = mTrajectoryQueue.front();
mTrajectoryQueue.pop();
mFuture = mExecutor->execute(std::move(traj));
mInProgress = true;
}
}
//==============================================================================
bool DartTNLP::get_bounds_info(Ipopt::Index n,
Ipopt::Number* x_l,
Ipopt::Number* x_u,
Ipopt::Index m,
Ipopt::Number* g_l,
Ipopt::Number* g_u)
{
const std::shared_ptr<Problem>& problem = mSolver->getProblem();
// here, the n and m we gave IPOPT in get_nlp_info are passed back to us.
// If desired, we could assert to make sure they are what we think they are.
assert(static_cast<std::size_t>(n) == problem->getDimension());
assert(static_cast<std::size_t>(m) == problem->getNumEqConstraints()
+ problem->getNumIneqConstraints());
DART_UNUSED(m);
// lower and upper bounds
for (Ipopt::Index i = 0; i < n; i++)
{
x_l[i] = problem->getLowerBounds()[i];
x_u[i] = problem->getUpperBounds()[i];
}
// Add inequality constraint functions
std::size_t idx = 0;
for (std::size_t i = 0; i < problem->getNumEqConstraints(); ++i)
{
g_l[idx] = g_u[idx] = 0.0;
++idx;
}
for (std::size_t i = 0; i < problem->getNumIneqConstraints(); ++i)
{
// Ipopt interprets any number greater than nlp_upper_bound_inf as
// infinity. The default value of nlp_upper_bound_inf and
// nlp_lower_bound_inf is 1e+19 and can be changed through ipopt options.
g_l[idx] = -std::numeric_limits<double>::infinity();
g_u[idx] = 0;
idx++;
}
return true;
}
//==============================================================================
bool DartTNLP::eval_g(Ipopt::Index _n,
const Ipopt::Number* _x,
bool _new_x,
Ipopt::Index _m,
Ipopt::Number* _g)
{
const std::shared_ptr<Problem>& problem = mSolver->getProblem();
assert(static_cast<std::size_t>(_m) == problem->getNumEqConstraints()
+ problem->getNumIneqConstraints());
DART_UNUSED(_m);
// TODO(JS):
if (_new_x)
{
}
Eigen::Map<const Eigen::VectorXd> x(_x, _n);
std::size_t idx = 0;
// Evaluate function values for equality constraints
for (std::size_t i = 0; i < problem->getNumEqConstraints(); ++i)
{
_g[idx] = problem->getEqConstraint(i)->eval(
static_cast<const Eigen::VectorXd&>(x));
idx++;
}
// Evaluate function values for inequality constraints
for (std::size_t i = 0; i < problem->getNumIneqConstraints(); ++i)
{
_g[idx] = problem->getIneqConstraint(i)->eval(
static_cast<const Eigen::VectorXd&>(x));
idx++;
}
return true;
}
bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A,
const Eigen::VectorXd& _b,
Eigen::VectorXd* _x,
int _numContacts,
double _mu,
int _numDir,
bool _bUseODESolver) {
if (!_bUseODESolver) {
int err = Lemke(_A, _b, _x);
return (err == 0);
} else {
assert(_numDir >= 4);
DART_UNUSED(_numDir);
double* A, *b, *x, *w, *lo, *hi;
int n = _A.rows();
int nSkip = dPAD(n);
A = new double[n * nSkip];
b = new double[n];
x = new double[n];
w = new double[n];
lo = new double[n];
hi = new double[n];
int* findex = new int[n];
memset(A, 0, n * nSkip * sizeof(double));
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
A[i * nSkip + j] = _A(i, j);
}
}
for (int i = 0; i < n; ++i) {
b[i] = -_b[i];
x[i] = w[i] = lo[i] = 0;
hi[i] = dInfinity;
findex[i] = -1;
}
for (int i = 0; i < _numContacts; ++i) {
findex[_numContacts + i * 2 + 0] = i;
findex[_numContacts + i * 2 + 1] = i;
lo[_numContacts + i * 2 + 0] = -_mu;
lo[_numContacts + i * 2 + 1] = -_mu;
hi[_numContacts + i * 2 + 0] = _mu;
hi[_numContacts + i * 2 + 1] = _mu;
}
// dClearUpperTriangle (A,n);
dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
// for (int i = 0; i < n; i++) {
// if (w[i] < 0.0 && abs(x[i] - hi[i]) > 0.000001)
// cout << "w[" << i << "] is negative, but x is " << x[i] << endl;
// else if (w[i] > 0.0 && abs(x[i] - lo[i]) > 0.000001)
// cout << "w[" << i << "] is positive, but x is " << x[i]
// << " lo is " << lo[i] << endl;
// else if (abs(w[i]) < 0.000001 && (x[i] > hi[i] || x[i] < lo[i]))
// cout << "w[i] " << i << " is zero, but x is " << x[i] << endl;
// }
*_x = Eigen::VectorXd(n);
for (int i = 0; i < n; ++i) {
(*_x)[i] = x[i];
}
// checkIfSolution(reducedA, reducedb, _x);
delete[] A;
delete[] b;
delete[] x;
delete[] w;
delete[] lo;
delete[] hi;
delete[] findex;
return 1;
}
}
