double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}
예제 #2
0
void
LinearIsoElasticPFDamage::computeQpElasticityTensor()
{
  _elasticity_tensor[_qp] = _Cijkl;

  getScalingFactor();

  if ( _scaling < 0.0 )
    mooseError("LinearIsoElasticPFDamage: scaling parameter cannot be negative");
  /**
   * Avoids zeroing of elasticity tensor if scaling is 0.0 (void with zero stiffness)
   * Adds user specified small number kdamage
   */
  _scaling += _kdamage;

  _elasticity_tensor[_qp] *= _scaling;
  _Jacobian_mult[_qp] = _elasticity_tensor[_qp];
}
예제 #3
0
파일: GUI.cpp 프로젝트: smistad/FAST
GUI::GUI() {
    mStreamingMode = STREAMING_MODE_STORE_ALL_FRAMES;
    QScreen* screen = QGuiApplication::primaryScreen();
    menuWidth = screen->geometry().width()*(1.0f/6.0f);
    mPipelineWidget = nullptr;
    mStreamer = ImageFileStreamer::New();

    QVBoxLayout* viewLayout = new QVBoxLayout;


    View* view = createView();
    view->set2DMode();
    view->setBackgroundColor(Color::Black());
    setWidth(screen->geometry().width());
    setHeight(screen->geometry().height());
    enableMaximized();
    setTitle("FAST - Viewer");
    viewLayout->addWidget(view);

    menuLayout = new QVBoxLayout;
    menuLayout->setAlignment(Qt::AlignTop);

    // Logo
    QImage* image = new QImage;
    image->load((Config::getDocumentationPath() + "images/FAST_logo_square.png").c_str());
    QLabel* logo = new QLabel;
    logo->setPixmap(QPixmap::fromImage(image->scaled(menuWidth, ((float)menuWidth/image->width())*image->height(), Qt::IgnoreAspectRatio, Qt::SmoothTransformation)));
    logo->adjustSize();
    menuLayout->addWidget(logo);

    // Title label
    QLabel* title = new QLabel;
    title->setText("Viewer");
	QFont font;
	font.setPixelSize(24 * getScalingFactor());
	font.setWeight(QFont::Bold);
	title->setFont(font);
	title->setAlignment(Qt::AlignCenter);
    menuLayout->addWidget(title);

    // Quit button
    QPushButton* quitButton = new QPushButton;
    quitButton->setText("Quit (q)");
    quitButton->setStyleSheet("QPushButton { background-color: red; color: white; }");
    quitButton->setFixedWidth(menuWidth);
    menuLayout->addWidget(quitButton);

    // Connect the clicked signal of the quit button to the stop method for the window
    QObject::connect(quitButton, &QPushButton::clicked, std::bind(&Window::stop, this));

    QLabel* inputListLabel = new QLabel;
    //inputListLabel->setFixedHeight(24);
    inputListLabel->setText("Input data");
    inputListLabel->setStyleSheet("QLabel { font-weight: bold; }");
    menuLayout->addWidget(inputListLabel);

    mList = new QListWidget;
    mList->setFixedWidth(menuWidth);
    mList->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
    mList->setFixedHeight(200);
    mList->setSelectionMode(QAbstractItemView::ExtendedSelection); // Allow multiple items to be selected
    QObject::connect(mList, &QListWidget::itemSelectionChanged, std::bind(&GUI::selectInputData, this));
    menuLayout->addWidget(mList);

    QPushButton* addButton = new QPushButton;
    addButton->setText("Add input data");
    addButton->setFixedWidth(menuWidth);
    QObject::connect(addButton, &QPushButton::clicked, std::bind(&GUI::addInputData, this));
    menuLayout->addWidget(addButton);

    QLabel* selectPipelineLabel = new QLabel;
    selectPipelineLabel->setText("Active pipeline");
    selectPipelineLabel->setStyleSheet("QLabel { font-weight: bold; }");
    //selectPipelineLabel->setFixedHeight(24);
    menuLayout->addWidget(selectPipelineLabel);

    mSelectPipeline = new QComboBox;
    mSelectPipeline->setFixedWidth(menuWidth);
    mPipelines = getAvailablePipelines();
    int index = 0;
    int counter = 0;
    for(auto pipeline : mPipelines) {
        mSelectPipeline->addItem((pipeline.getName() + " (" + pipeline.getDescription() + ")").c_str());
        if(pipeline.getName() == "Image renderer") {
            index = counter;
        }
        ++counter;
    }
    mSelectPipeline->setCurrentIndex(index);

    menuLayout->addWidget(mSelectPipeline);
    QObject::connect(mSelectPipeline, static_cast<void (QComboBox::*)(int)>(&QComboBox::activated), std::bind(&GUI::selectPipeline, this));

    QPushButton* refreshPipeline = new QPushButton;
    refreshPipeline->setText("Refresh pipeline");
    refreshPipeline->setStyleSheet("QPushButton { background-color: blue; color: white; }");
    refreshPipeline->setFixedWidth(menuWidth);
    QObject::connect(refreshPipeline, &QPushButton::clicked, std::bind(&GUI::selectPipeline, this));
    menuLayout->addWidget(refreshPipeline);

    QPushButton* editPipeline = new QPushButton;
    editPipeline->setText("Edit pipeline");
    editPipeline->setStyleSheet("QPushButton { background-color: blue; color: white; }");
    editPipeline->setFixedWidth(menuWidth);
    QObject::connect(editPipeline, &QPushButton::clicked, std::bind(&GUI::editPipeline, this));
    menuLayout->addWidget(editPipeline);

    QPushButton* newPipeline = new QPushButton;
    newPipeline->setText("New pipeline");
    newPipeline->setStyleSheet("QPushButton { background-color: blue; color: white; }");
    newPipeline->setFixedWidth(menuWidth);
    QObject::connect(newPipeline, &QPushButton::clicked, std::bind(&GUI::newPipeline, this));
    menuLayout->addWidget(newPipeline);

    // Playback
    QHBoxLayout* playbackLayout = new QHBoxLayout;

    mPlayPauseButton = new QPushButton;
    mPlayPauseButton->setText("Play");
    mPlayPauseButton->setStyleSheet("QPushButton { background-color: green; color: white; }");
    //mPlayPauseButton->setFixedHeight(100);
    QObject::connect(mPlayPauseButton, &QPushButton::clicked, std::bind(&GUI::playPause, this));
    playbackLayout->addWidget(mPlayPauseButton);

    mTimestepSlider = new QSlider(Qt::Horizontal);
    // Improve style of slider
    mTimestepSlider->setStyleSheet("QSlider:horizontal { min-height: 50px; } QSlider::groove:horizontal {\n"
                                           "    border: 1px solid #999999;\n"
                                           "    height: 10px; /* the groove expands to the size of the slider by default. by giving it a height, it has a fixed size */\n"
                                           "    background: qlineargradient(x1:0, y1:0, x2:0, y2:1, stop:0 #B1B1B1, stop:1 #c4c4c4);\n"
                                           "    margin: 2px 0;\n"
                                           "}\n"
                                           "\n"
                                           "QSlider::handle:horizontal {\n"
                                           "    background: qlineargradient(x1:0, y1:0, x2:1, y2:1, stop:0 #81BEF7, stop:1 #2E9AFE);\n"
                                           "    border: 1px solid #5c5c5c;\n"
                                           "    width: 18px;\n"
                                           "    height: 25px;\n"
                                           "    margin: -10px 0; /* handle is placed by default on the contents rect of the groove. Expand outside the groove */\n"
                                           "    border-radius: 3px;\n"
                                           "}");
    playbackLayout->addWidget(mTimestepSlider);
    mTimestepSlider->setTickPosition(QSlider::NoTicks);
    mTimestepSlider->setRange(0, 1234);
    mTimestepSlider->setPageStep(1);
    mTimestepSlider->setSingleStep(1);
    mTimestepSlider->setStyle(new MyStyle(mTimestepSlider->style())); // Fixes issues with direct jump with slider
    QObject::connect(mTimestepSlider, &QSlider::sliderMoved, std::bind(&GUI::setTimestep, this));
    QObject::connect(mTimestepSlider, &QSlider::sliderPressed, std::bind(&GUI::setTimestep, this));

    viewLayout->addLayout(playbackLayout);

    // Add menu and view to main layout
    QHBoxLayout* layout = new QHBoxLayout;
    layout->addLayout(menuLayout);
    layout->addLayout(viewLayout);

    mWidget->setLayout(layout);
	std::cout << "Finished viewer setup" << std::endl;

}