Source

papl_robwork / c_robwork / papl_robwork.cpp

/*
  Copyright (c) 2012 Anders Lau Olsen.
  See LICENSE file for terms and conditions.
*/

#include "papl_robwork.h"
#include "papl_robwork.hpp"
#include <rw/math/Transform3D.hpp>
#include <rw/math/Rotation3D.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/common/macros.hpp>
#include <rw/common/Exception.hpp>
#include <rw/kinematics/Kinematics.hpp>
#include <rw/math/Q.hpp>
#include <rw/loaders/WorkCellFactory.hpp>
#include <rw/loaders/path/PathLoader.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyYaobi.hpp>
#include <sstream>
#include <algorithm>

using namespace rw::models;
using namespace rw::math;
using namespace rw::kinematics;
using namespace rw::loaders;
using namespace rw::common;
using namespace rw::proximity;
using namespace rw::trajectory;
using namespace rwlibs::proximitystrategies;

/*----------------------------------------------------------------------*
  Destructors
 *----------------------------------------------------------------------*/

void robwork_freeWorkCell(RW_WorkCell* workcell)
{
    delete workcell;
}

void robwork_freeState(RW_State* state)
{
    delete state;
}

void robwork_freeDevice(RW_Device* device)
{
    delete device;
}

void robwork_freeFrame(RW_Frame* frame)
{
    delete frame;
}

void robwork_freeFKTable(RW_FKTable* table)
{
    delete table;
}

void robwork_freeTimedStatePath(RW_TimedStatePath* path)
{
    delete path;
}

/*----------------------------------------------------------------------*
  Data verification.
 *----------------------------------------------------------------------*/

const char* robwork_getWorkCellError(const RW_WorkCell* workcell)
{
    RW_ASSERT(workcell);

    return workcell->error.c_str();
}

int robwork_getWorkCellErrorSize(const RW_WorkCell* workcell)
{
    return (int)workcell->error.size();
}

int robwork_isValidDevice(const RW_Device* device)
{
    return device->device != NULL;
}

int robwork_isValidWorkCell(const RW_WorkCell* workcell)
{
    return workcell->workcell != NULL;
}

int robwork_isValidFrame(const RW_Frame* frame)
{
    return frame->frame != NULL;
}

/*----------------------------------------------------------------------*
  Workcell operations
 *----------------------------------------------------------------------*/

RW_WorkCell* robwork_loadWorkCell(const char* file)
{
    RW_ASSERT(file);

    try {
        WorkCell::Ptr workcell = WorkCellFactory::load(file);

        // The RobWork API doc claims that an exception is thrown if the
        // workcell couldn't be loaded, but that is not true.
        if (workcell) {
            CollisionStrategy::Ptr strategy = ProximityStrategyYaobi::make();
            CollisionDetector::Ptr detector =
                ownedPtr(new CollisionDetector(workcell, strategy));

            return new RW_WorkCell(workcell, detector, "");
        } else {
            return new RW_WorkCell(
                0, 0, std::string("Can't load ") + file);
        }
    } catch (const Exception& e) {
        // Because of the RobWork issue above, this branch is in practice never
        // being executed.
        std::ostringstream buf;
        buf << e;
        return new RW_WorkCell(0, 0, buf.str());
    }
}

RW_State* robwork_getState(const RW_WorkCell* workcell)
{
    RW_ASSERT(workcell);

    return new RW_State(workcell->workcell->getDefaultState());
}

RW_Device* robwork_getDevice(const RW_WorkCell* workcell, const char* name)
{
    RW_ASSERT(workcell);

    return new RW_Device(workcell->workcell->findDevice(name));
}

RW_Frame* robwork_getFrame(const RW_WorkCell* workcell, const char* name)
{
    RW_ASSERT(workcell);

    return new RW_Frame(workcell->workcell->findFrame(name));
}

/*----------------------------------------------------------------------*
  Device operations
 *----------------------------------------------------------------------*/

int robwork_getDOF(const RW_Device* device)
{
    RW_ASSERT(device);

    return (int)device->device->getDOF();
}

namespace
{
    void writeQ(double* vals, const Q& q)
    {
        std::copy(q.begin(), q.begin() + q.size(), vals);
    }
}

void robwork_getVelocityLimits(const RW_Device* device, double* vals)
{
    RW_ASSERT(device);
    RW_ASSERT(vals);

    writeQ(vals, device->device->getVelocityLimits());
}

void robwork_getLower(const RW_Device* device, double* vals)
{
    RW_ASSERT(device);
    RW_ASSERT(vals);

    writeQ(vals, device->device->getBounds().first);
}

void robwork_getUpper(const RW_Device* device, double* vals)
{
    RW_ASSERT(device);
    RW_ASSERT(vals);

    writeQ(vals, device->device->getBounds().second);
}

RW_Frame* robwork_getEnd(const RW_Device* device)
{
    RW_ASSERT(device);

    return new RW_Frame(device->device->getEnd());
}

/*----------------------------------------------------------------------*
  State operations
 *----------------------------------------------------------------------*/

RW_State* robwork_copyState(const RW_State* state)
{
    RW_ASSERT(state);

    return new RW_State(state->state);
}

RW_FKTable* robwork_getFKTable(const RW_State* state)
{
    RW_ASSERT(state);

    return new RW_FKTable(ownedPtr(new FKTable(state->state)));
}

/*----------------------------------------------------------------------*
  FKTable operations
 *----------------------------------------------------------------------*/

void robwork_getTransform(
    const RW_FKTable* table,
    const RW_Frame* frame,
    double* out)
{
    RW_ASSERT(table);
    RW_ASSERT(frame);
    RW_ASSERT(out);

    const Transform3D<>& t = table->table->get(frame->frame);
    const Vector3D<>& p = t.P();
    const Rotation3D<>& r = t.R();

    out[0] = r(0, 0); out[1] = r(0, 1); out[2]  = r(0, 2); out[3]  = p(0);
    out[4] = r(1, 0); out[5] = r(1, 1); out[6]  = r(1, 2); out[7]  = p(1);
    out[8] = r(2, 0); out[9] = r(2, 1); out[10] = r(2, 2); out[11] = p(2);
}

/*----------------------------------------------------------------------*
  Device and state operations
 *----------------------------------------------------------------------*/

void robwork_setQ(
    RW_State* state,
    const RW_Device* device,
    const double* vals)
{
    RW_ASSERT(device);
    RW_ASSERT(state);
    RW_ASSERT(vals);

    device->device->setQ(Q(device->device->getDOF(), vals), state->state);
}

void robwork_getQ(
    const RW_State* state,
    const RW_Device* device,
    double* vals)
{
    RW_ASSERT(device);
    RW_ASSERT(state);
    RW_ASSERT(vals);

    const Q q = device->device->getQ(state->state);
    writeQ(vals, q);
}

/*----------------------------------------------------------------------*
  Frame operations
 *----------------------------------------------------------------------*/

int robwork_gripFrame(
    RW_Frame* item, RW_Frame* gripper, RW_State* state)
{
    RW_ASSERT(item);
    RW_ASSERT(gripper);
    RW_ASSERT(state);

    try {
        Kinematics::gripFrame(item->frame, gripper->frame, state->state);
        return 1;
    } catch (const Exception& e) {
        // There is a useful error message in [e] but it is too much work
        // passing it on.
        return 0;
    }
}

/*----------------------------------------------------------------------*
  Collision detection
 *----------------------------------------------------------------------*/

int robwork_inCollision(const RW_WorkCell* workcell, const RW_State* state)
{
    RW_ASSERT(state);
    RW_ASSERT(workcell);

    return (int)workcell->detector->inCollision(
        state->state, 0, true);
}

/*----------------------------------------------------------------------*
  (Time, State) path operations
 *----------------------------------------------------------------------*/

RW_TimedStatePath* robwork_makeTimedStatePath()
{
    return new RW_TimedStatePath();
}

void robwork_addTimedState(
    RW_TimedStatePath* path, double time, const RW_State* state)
{
    RW_ASSERT(path);
    RW_ASSERT(state);

    path->path.push_back(Timed<State>(time, state->state));
}

int robwork_writeTimedStatePath(
    const RW_WorkCell* workcell,
    const RW_TimedStatePath* path,
    const char* file)
{
    RW_ASSERT(workcell);
    RW_ASSERT(path);
    RW_ASSERT(file);

    try {
        PathLoader::storeTimedStatePath(
            *workcell->workcell,
            path->path,
            file);
        return 1;
    } catch (const Exception& e) {
        return 0;
    }
}