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;
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 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);
}
}
