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({qvar},con1));
prog.addConstraint(con1wrapper);
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({qvar},con2));
prog.addConstraint(con2wrapper);
}
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;
}
