jointwidget.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas */

#include <se2ez/core/log.h>
#include <se2ez/core/joint.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/state.h>
#include <se2ez/core/math.h>

#include <se2ez/gui/jointpanel.h>
#include <se2ez/gui/jointwidget.h>

#include "ui_jointwidget.h"

using namespace se2ez;

gui::JointWidget::JointWidget(Panel *parent, std::string name, RobotPtr robot, StatePtr state)
  : QWidget(parent), state_(state), robot_(robot), ui_(new Ui::JointWidget), panel_(parent)
{
    ui_->setupUi(this);

    name_ = name;
    ui_->name->setText(tr(name.c_str()));
    // vars  {x, y, t}
    value_ = {0, 0, 0};
    lower_ = {0, 0, 0};
    upper_ = {0, 0, 0};

    const auto &frame = robot_->getFrame(name);
    const auto &joint = frame->getJoint();

    const auto &bounds = joint.getBounds();

    switch (joint.type)
    {
        case Joint::FLOAT:
        case Joint::FLYING:
            lower_ = bounds.first;
            upper_ = bounds.second;
            break;
        case Joint::TRANSLATE:
            lower_.head(2) = bounds.first;
            upper_.head(2) = bounds.second;
            ui_->tWidget->hide();
            break;
        case Joint::PRISMATIC:
            ui_->yWidget->hide();
            ui_->tWidget->hide();
            lower_[0] = bounds.first[0];
            upper_[0] = bounds.second[0];
            break;
        case Joint::REVOLUTE:
        case Joint::CONTINUOUS:
            ui_->xWidget->hide();
            ui_->yWidget->hide();
            lower_[2] = bounds.first[0];
            upper_[2] = bounds.second[0];
            break;
        default:
            break;
    }

    // Connect the slider signals
    sliders[0] = ui_->xSlider;
    sliders[1] = ui_->ySlider;
    sliders[2] = ui_->tSlider;
    connect(ui_->xSlider, SIGNAL(valueChanged(int)), this, SLOT(xChanged(int)));
    connect(ui_->ySlider, SIGNAL(valueChanged(int)), this, SLOT(yChanged(int)));
    connect(ui_->tSlider, SIGNAL(valueChanged(int)), this, SLOT(tChanged(int)));

    // Connect the box signals
    boxes[0] = ui_->xSpinBox;
    boxes[1] = ui_->ySpinBox;
    boxes[2] = ui_->tSpinBox;
    connect(ui_->xSpinBox, SIGNAL(valueChanged(double)), this, SLOT(xBoxChanged(double)));
    connect(ui_->ySpinBox, SIGNAL(valueChanged(double)), this, SLOT(yBoxChanged(double)));
    connect(ui_->tSpinBox, SIGNAL(valueChanged(double)), this, SLOT(tBoxChanged(double)));

    // Set limits on the spinBoxes
    for (unsigned int i = 0; i < 3; ++i)
    {
        boxes[i]->setMinimum(lower_[i]);
        boxes[i]->setMaximum(upper_[i]);
    }

    reset();
}

// TODO:remove or fix to make better
void gui::JointWidget::keyPressEvent(QKeyEvent *event)
{
    switch (event->key())
    {
        case Qt::Key_D:
            boxes[0]->setValue(boxes[0]->value() + boxes[0]->singleStep());
            break;
        case Qt::Key_A:
            boxes[0]->setValue(boxes[0]->value() - boxes[0]->singleStep());
            break;
        case Qt::Key_S:
            boxes[1]->setValue(boxes[1]->value() - boxes[1]->singleStep());
            break;
        case Qt::Key_W:
            boxes[1]->setValue(boxes[1]->value() + boxes[1]->singleStep());
            break;
        case Qt::Key_J:
            boxes[2]->setValue(boxes[2]->value() + boxes[2]->singleStep());
            break;
        case Qt::Key_K:
            boxes[2]->setValue(boxes[2]->value() - boxes[2]->singleStep());
            break;
        default:
            QWidget::keyPressEvent(event);
    }
}

void gui::JointWidget::setValue()
{
    for (unsigned int i = 0; i < 3; ++i)
    {
        setBox(i, value_[i], false);
        setSlider(i, sliders[i]->value(), false);
    }
}

void gui::JointWidget::setSlider(unsigned int index, int value, bool update)
{
    value_[index] = math::remap(sliders[index]->minimum(), sliders[index]->maximum(),  //
                                value,                                                 //
                                lower_[index], upper_[index]);

    boxes[index]->blockSignals(true);
    boxes[index]->setValue(value_[index]);
    boxes[index]->blockSignals(false);

    if (update)
        updateState();
}

void gui::JointWidget::setBox(unsigned int index, double value, bool update)
{
    value_[index] = value;

    sliders[index]->blockSignals(true);
    sliders[index]->setValue(math::remap(lower_[index], upper_[index],  //
                                         value_[index],                 //
                                         sliders[index]->minimum(), sliders[index]->maximum() + 0.5));
    sliders[index]->blockSignals(false);

    if (update)
        updateState();
}

void gui::JointWidget::xChanged(int value)
{
    setSlider(0, value, true);
}

void gui::JointWidget::yChanged(int value)
{
    setSlider(1, value, true);
}

void gui::JointWidget::tChanged(int value)
{
    setSlider(2, value, true);
}

void gui::JointWidget::xBoxChanged(double value)
{
    setBox(0, value, true);
}

void gui::JointWidget::yBoxChanged(double value)
{
    setBox(1, value, true);
}

void gui::JointWidget::tBoxChanged(double value)
{
    setBox(2, value, true);
}

void gui::JointWidget::fromState()
{
    std::lock_guard<std::recursive_mutex> lock(panel_->mutex_);

    const auto &value = robot_->getFrameValues(name_, state_->data);
    switch (robot_->getFrame(name_)->getJoint().type)
    {
        case Joint::FLOAT:
        case Joint::FLYING:
            value_ = value;
            break;
        case Joint::TRANSLATE:
            value_.head(2) = value;
            break;
        case Joint::PRISMATIC:
            value_[0] = value[0];
            break;
        case Joint::REVOLUTE:
        case Joint::CONTINUOUS:
            value_[2] = value[0];
            break;
        default:
            break;
    }

    setValue();

    emit stateChanged();
}

void gui::JointWidget::updateState()
{
    std::lock_guard<std::recursive_mutex> lock(panel_->mutex_);

    // Update the state in canvas
    switch (robot_->getFrame(name_)->getJoint().type)
    {
        case Joint::FLOAT:
        case Joint::FLYING:
            state_->setFloatJoint(robot_, name_, Eigen::Vector3d(value_[0], value_[1], value_[2]));
            break;
        case Joint::TRANSLATE:
            state_->setTranslateJoint(robot_, name_, Eigen::Vector2d(value_[0], value_[1]));
            break;
        case Joint::PRISMATIC:
            state_->setJoint(robot_, name_, value_[0]);
            break;
        case Joint::REVOLUTE:
        case Joint::CONTINUOUS:
            state_->setJoint(robot_, name_, value_[2]);
            break;
        default:
            break;
    }

    emit stateChanged();
}

void gui::JointWidget::reset()
{
    ui_->xSpinBox->setValue(lower_[0] + fabs(upper_[0] - lower_[0]) / 2.);
    ui_->ySpinBox->setValue(lower_[1] + fabs(upper_[1] - lower_[1]) / 2.);
    ui_->tSpinBox->setValue(lower_[2] + fabs(upper_[2] - lower_[2]) / 2.);
    ui_->xSlider->setValue(50);
    ui_->ySlider->setValue(50);
    ui_->tSlider->setValue(50);

    updateState();
}

gui::JointWidget::~JointWidget()
{
    delete ui_;
}