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