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