jointpanel.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas, Zachary Kingston */

#include <se2ez/core/log.h>
#include <se2ez/core/io.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/ik.h>
#include <se2ez/core/box2d/collision.h>

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

#include "ui_jointpanel.h"

using namespace se2ez;

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

    // allocating space for state
    jstate_ = std::make_shared<State>(robot_);

    for (const auto &name : robot_->getFrameNames())
    {
        const auto &data = robot_->getFrameDataConst(name);
        if (data->movable)
            ui_->ees->addItem(QString(name.c_str()));
    }

    ui_->ees->setCurrentIndex(ui_->ees->count() - 1);

    cm_ = std::make_shared<Box2DCollisionManager>(robot_);
    for (const auto &name : robot_->getFrameNames())
    {
        // We don't add jointWidgets for fixed joints
        if (!(robot_->getFrame(name)->getJoint().type == Joint::FIXED))
        {
            auto jw = new gui::JointWidget(this, name, robot_, jstate_);

            ui_->vbox->insertWidget(0, jw);
            connect(jw, SIGNAL(stateChanged()), this, SLOT(updateState()));

            joints_.emplace_back(jw);
        }
    };

    connect(ui_->reset, SIGNAL(clicked()), this, SLOT(reset()));
    connect(ui_->random, SIGNAL(clicked()), this, SLOT(random()));
    connect(ui_->randomCF, SIGNAL(clicked()), this, SLOT(randomCollisionFree()));
    connect(ui_->frames, SIGNAL(stateChanged(int)), this, SLOT(updateState()));
    connect(ui_->geometry, SIGNAL(stateChanged(int)), this, SLOT(updateState()));
    connect(ui_->collide, SIGNAL(stateChanged(int)), this, SLOT(updateState()));
    connect(ui_->sd, SIGNAL(stateChanged(int)), this, SLOT(updateState()));
    connect(ui_->aware, SIGNAL(stateChanged(int)), this, SLOT(updateIK()));
    connect(ui_->ees, SIGNAL(currentIndexChanged(const QString &)), this, SLOT(updateIK()));

    updateIK();
    reset();
}

void gui::JointPanel::updateIK()
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    ee_ = ui_->ees->currentText().toStdString();
    if (ui_->aware->isChecked())
        ik_ = std::make_shared<CollisionAwareChainIK>(robot_, ee_);
    else
        ik_ = std::make_shared<ChainIK>(robot_, ee_);
}

void gui::JointPanel::updateState()
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    if (ui_->geometry->isChecked())
    {
        if (ui_->sd->isChecked())
            minimum_ = cm_->distance(jstate_, distances_);

        else if (ui_->collide->isChecked())
            cm_->collide(jstate_, colliding_);
    }

    emit stateChanged();
}

void gui::JointPanel::setState(const StatePtr &state)
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    jstate_->copy(state);
    for (const auto &joint : joints_)
        joint->fromState();
}

void gui::JointPanel::getState(StatePtr state)
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    state->copy(jstate_);
}

const RobotPtr &gui::JointPanel::getRobot()
{
    return robot_;
}

const StatePtr &gui::JointPanel::getState()
{
    return jstate_;
}

const std::string &gui::JointPanel::getEE()
{
    return ee_;
}

std::string gui::JointPanel::getSignature()
{
    return robot_->getSignature();
}

void gui::JointPanel::reset()
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    for (const auto &joint : joints_)
        joint->reset();
}

void gui::JointPanel::random()
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    jstate_->setRandom(robot_);

    for (const auto &joint : joints_)
        joint->fromState();
}

void gui::JointPanel::randomCollisionFree()
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    unsigned int tries = 0;
    bool inCollision;

    do
    {
        jstate_->setRandom(robot_);
        inCollision = cm_->collide(jstate_);
    } while ((tries++ < 50) && inCollision);

    if (not inCollision)
        for (const auto &joint : joints_)
            joint->fromState();
}

void gui::JointPanel::draw(QPainter &painter, gui::RenderArea *canvas)
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);
    jstate_->fk(robot_);

    auto pose = tf::flattenIsometry(jstate_->getPose(robot_, ee_));

    bool geo = ui_->geometry->isChecked();
    if (geo)
    {
        if (ui_->sd->isChecked())
            canvas->drawGeometry(painter, robot_, jstate_,
                                 std::make_shared<RenderArea::SignedDistanceDrawer>(
                                     ui_->collide->isChecked(), ui_->sd->checkState() == Qt::PartiallyChecked,
                                     minimum_, distances_, robot_, jstate_));
        else
            canvas->drawGeometry(painter, robot_, jstate_,
                                 std::make_shared<RenderArea::CollideDrawer>(ui_->collide->isChecked(),
                                                                             colliding_, robot_, jstate_));
    }

    bool frames = ui_->frames->isChecked();
    if (frames)
        canvas->drawAllFrames(painter, robot_, jstate_);

    if (geo || frames)
    {
        painter.save();
        painter.resetTransform();

        int ax, ay;
        canvas->toScreen(pose[0], pose[1], ax, ay);
        painter.drawText(ax + 5, ay - 5, name);

        painter.restore();
    }
}

bool gui::JointPanel::click(QMouseEvent * /*event*/, double x, double y, int /*sx*/, int /*sy*/)
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    const std::string &name = ui_->ees->currentText().toStdString();
    if (ik_->solve(jstate_, {{name, tf::toIsometry(Eigen::Vector3d{x, y, 0})}}, {{name, IKSolver::POSITION}}))
        for (const auto &joint : joints_)
            joint->fromState();

    return true;
}

void gui::JointPanel::update(boost::posix_time::ptime /*last*/, boost::posix_time::ptime /*current*/)
{
}

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