bool FootstepGraph::isGoal(StatePtr state)
{
FootstepState::Ptr goal = getGoal(state->getLeg());
if (publish_progress_) {
jsk_footstep_msgs::FootstepArray msg;
msg.header.frame_id = "odom"; // TODO fixed frame_id
msg.header.stamp = ros::Time::now();
msg.footsteps.push_back(*state->toROSMsg());
pub_progress_.publish(msg);
}
Eigen::Affine3f pose = state->getPose();
Eigen::Affine3f goal_pose = goal->getPose();
Eigen::Affine3f transformation = pose.inverse() * goal_pose;
if ((parameters_.goal_pos_thr > transformation.translation().norm()) &&
(parameters_.goal_rot_thr > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()))) {
// check collision
if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
if (right_goal_state_->crossCheck(state)) {
return true;
}
} else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
if (left_goal_state_->crossCheck(state)) {
return true;
}
}
}
return false;
}
int main(int argc, char **argv){
ros::init (argc, argv, "move_arm_pose_goal");
ros::NodeHandle nh;
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_rarm("move_right_arm",true);
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_larm("move_left_arm",true);
ROS_INFO("Connecting to server...");
move_rarm.waitForServer();
move_larm.waitForServer();
ROS_INFO("Connected to server");
actualizeGoal(nh, move_rarm, getGoal(0,-0.7,1.2,0,0,0,1, true));
actualizeGoal(nh, move_rarm, getGoal(0.75,-0.2,0.8,0,0,0,1, true));
actualizeGoal(nh, move_rarm, getGoal(0.5,-0.35,0.35,0,0,0,1, true));
ros::shutdown();
}
void Server::stop(){
qDebug() << "Server::stop()";
waitForNewMessage(WAITING_FOR_MESSAGE::DONE);
emit emitOutput(QString("\nMATCH FINISHED"));
emit matchFinished(proverSM.getGoals(currentState));
QThread::currentThread()->msleep(500);
int nbPlayers = addresses.size();
QString message = QString("(stop ") % matchId % " ";
// Likely to be not necessary
if(stepCounter == 0){
message += "nil)";
}
else{
message += "( ";
for(QString move : moves){
message += QString("(") % move % ") ";
}
message += QString(") )");
}
for(int i = 0; i<nbPlayers; ++i){
networkConnections[i]->request(message, startclock);
emit outputPlayerMessage(i, QString("Score %1").arg(getGoal(i)));
}
}
void ompl::control::SimpleSetup::setup()
{
if (!configured_ || !si_->isSetup() || !planner_->isSetup())
{
if (!si_->isSetup())
si_->setup();
if (!planner_)
{
if (pa_)
planner_ = pa_(si_);
if (!planner_)
{
OMPL_INFORM("No planner specified. Using default.");
planner_ = getDefaultPlanner(getGoal());
}
}
planner_->setProblemDefinition(pdef_);
if (!planner_->isSetup())
planner_->setup();
params_.clear();
params_.include(si_->params());
params_.include(planner_->params());
configured_ = true;
}
}
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec)
{
const ob::StateSpacePtr& space = getStateSpace();
space->setup();
// setup start state
ob::ScopedState<> start(space);
if (stateVec.size() == space->getDimension())
space->copyFromReals(start.get(), stateVec);
else
{
// Pick koule positions evenly radially distributed, but at a linearly
// increasing distance from the center. The ship's initial position is
// at the center. Initial velocities are 0.
std::vector<double> startVec(space->getDimension(), 0.);
double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
startVec[0] = .5 * sideLength;
startVec[1] = .5 * sideLength;
startVec[4] = .5 * delta;
for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
{
r = .1 + i * .1 / numKoules;
startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
}
space->copyFromReals(start.get(), startVec);
}
setStartState(start);
// set goal
setGoal(std::make_shared<KoulesGoal>(si_));
// set propagation step size
si_->setPropagationStepSize(propagationStepSize);
// set min/max propagation steps
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
bool isPDST = plannerName == "pdst";
const ompl::base::GoalPtr& goal = getGoal();
si_->setDirectedControlSamplerAllocator(
[&goal, isPDST](const ompl::control::SpaceInformation *si)
{
return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
});
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
// set state propagator
setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
bool FootstepGraph::isGoal(StatePtr state)
{
FootstepState::Ptr goal = getGoal(state->getLeg());
if (publish_progress_) {
jsk_footstep_msgs::FootstepArray msg;
msg.header.frame_id = "odom";
msg.header.stamp = ros::Time::now();
msg.footsteps.push_back(*state->toROSMsg());
pub_progress_.publish(msg);
}
Eigen::Affine3f pose = state->getPose();
Eigen::Affine3f goal_pose = goal->getPose();
Eigen::Affine3f transformation = pose.inverse() * goal_pose;
return (pos_goal_thr_ > transformation.translation().norm()) &&
(rot_goal_thr_ > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()));
}
inline std::ostream& FileConfig::print(std::ostream& os) const
{
return os <<
"bruijn=" << getBruijn() << "\n" <<
"emch=" << getEmch() << "\n" <<
"outputFormat=" << getOutputFormat() << "\n" <<
"fitFilter=" << getFitFilter() << "\n" <<
"goal=" << getGoal() << "\n" <<
"info=" << getInfo() << "\n" <<
"mch=" << getMch() << "\n" <<
"orderingHeuristicConfig=" << getOrderingHeuristicConfig() << "\n" <<
"parityBacktrack=" << getParityBacktrack() << "\n" <<
"parityFilter=" << getParityFilter() << "\n" <<
"quiet=" << getQuiet() << "\n" <<
"redundancyFilter=" << getRedundancyFilter() << "\n" <<
"redundancyFilterFirst=" << getRedundancyFilterFirst() << "\n" <<
"sample=" << getMonteCarlo() << "\n" <<
"trace=" << getTrace() << "\n" <<
"unique=" << getUnique() << "\n" <<
"volumeBacktrack=" << getVolumeBacktrack() << "\n" <<
"volumeFilter=" << getVolumeFilter() << std::endl;
}
void ompl::geometric::SimpleSetup::setup()
{
if (!configured_ || !si_->isSetup() || !planner_->isSetup())
{
if (!si_->isSetup())
si_->setup();
if (!planner_)
{
if (pa_)
planner_ = pa_(si_);
if (!planner_)
{
OMPL_INFORM("No planner specified. Using default.");
planner_ = tools::SelfConfig::getDefaultPlanner(getGoal());
}
}
planner_->setProblemDefinition(pdef_);
if (!planner_->isSetup())
planner_->setup();
configured_ = true;
}
}
//check whether the game is over
int checkGameState(Agent agents[],Graph g,int cycle,int maxCycles){
if(cycle >= maxCycles) {
printf("GAME OVER: YOU LOSE - TIME IS UP\n");
return OVER;
} else {
Vertex currThiefLoc = getCurrentLocation (agents[THIEF]);
int i = 1;
while (i <= NUM_DETECTIVES) {
if (currThiefLoc == getCurrentLocation (agents[i])) {
printf("D%d caught the thief in %s (%d)\n", i, getCityName(g, currThiefLoc), currThiefLoc);
printf("YOU WIN - THIEF CAUGHT!\n");
return WIN;
}
i++;
}
if ( currThiefLoc == getGoal (agents[THIEF]) ) {
printf("T got away to %s (%d)\n", getCityName(g, currThiefLoc), currThiefLoc);
printf("GAME OVER: YOU LOSE - THIEF GOT TO GETAWAY\n");
return LOSE;
}
return CONTINUE;
}
}
int main(int argc, char *argv[]) {
Maze maze;
readMazeFromFile(&maze, "maze3D.txt");
unsigned int solution_count = 13;
Direction solution[] = {EAST, NORTH, EAST, SOUTH, EAST, NORTH, UP, WEST, WEST, NORTH, NORTH, DOWN, SOUTH};
Position position, goal;
getPosition(maze, &position);
getGoal(maze, &goal);
printf("Current position: "); printPositionWithNewline(position);
printf("Goal: "); printPositionWithNewline(goal); printf("\n");
printMaze(maze);
printPossibleMoves(maze);
printf("\n");
for (int i=0; i < solution_count; i++) {
if (makeMove(&maze, solution[i]) == 1) {
if (isSolved(maze)) {
printf("Maze solved!\n");
printPathWithNewline(maze.path);
break;
}
else {
printf("Movement successful, maze not solved.\n");
}
}
else {
printf("Error moving from: "); printPosition(maze.pos); printf (" to %s\n", convertDirectionToString(solution[i]));
}
}
return EXIT_SUCCESS;
}
bool parallelPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners, unsigned int links,
unsigned int chains, struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
{
// Create a shared pointer to our constraint.
auto constraint = std::make_shared<ParallelConstraint>(links, chains);
ConstrainedProblem cp(space, constraint->createSpace(), constraint);
cp.setConstrainedOptions(c_opt);
cp.setAtlasOptions(a_opt);
cp.css->registerProjection("parallel", constraint->getProjection(cp.css));
Eigen::VectorXd start, goal;
constraint->getStart(start);
constraint->getGoal(goal);
cp.setStartAndGoalStates(start, goal);
cp.ss->setStateValidityChecker(std::bind(&ParallelConstraint::isValid, constraint, std::placeholders::_1));
if (!bench)
return parallelPlanningOnce(cp, planners[0], output);
else
return parallelPlanningBench(cp, planners);
}
void Arcade::updateBoard(float dt){
if(_stop) return;
if(!_in_combo && _time > 0){
switch(getGoal()){
case SCORE_WIN:
if(_score > getScoreLowWin()){
unlockNextLevel();
showWinner();
return;
}
break;
case COMBO_WIN:
if(_combo_count > getComboWin()){
showWinner();
unlockNextLevel();
}
break;
}
}
_time_roll_board.update(dt);
_time_combo.update(dt);
updateClock(dt);
updateTutorial(dt);
}
