state.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

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

using namespace se2ez;

State::State(const Robot &robot)
  : data(Eigen::VectorXd::Zero(robot.getNumJoints())), frames(std::make_shared<FrameMap>(robot))
{
    // Setting the state to 0 could be out of bounds
    for (const auto &pair : robot.getJoints())
    {
        const auto &frame = pair.second;
        const auto &joint = frame->frame->getJoint();

        joint.enforceBounds(data);
    }
}

State::State(const RobotPtr &robot)
  : data(Eigen::VectorXd::Zero(robot->getNumJoints())), frames(std::make_shared<FrameMap>(robot))
{
    // Setting the state to 0 could be out of bounds
    for (const auto &pair : robot->getJoints())
    {
        const auto &frame = pair.second;
        const auto &joint = frame->frame->getJoint();

        joint.enforceBounds(data);
    }
}

void State::setJoint(const RobotPtr &robot, const std::string &name, double value)
{
    robot->getFrameValues(name, data)[0] = value;
    dirty = true;
}

void State::setTranslateJoint(const RobotPtr &robot, const std::string &name, const Eigen::Vector2d &value)
{
    robot->getFrameValues(name, data) = value;
    dirty = true;
}

void State::setFloatJoint(const RobotPtr &robot, const std::string &name, const Eigen::Isometry2d &value)
{
    setFloatJoint(robot, name, tf::flattenIsometry(value));
    dirty = true;
}

void State::setFloatJoint(const RobotPtr &robot, const std::string &name, const Eigen::Vector3d &value)
{
    robot->getFrameValues(name, data) = value;
    dirty = true;
}

void State::setRandom(const RobotPtr &robot)
{
    for (const auto &fname : robot->getFrameNames())
    {
        const auto &frameData = robot->getFrameDataConst(fname);
        frameData->frame->getJoint().setRandom(frameData->getValues(data));
    }

    dirty = true;
}

void State::copy(const StatePtr &other)
{
    data = other->data;
    dirty = true;
}

void State::fk(const RobotPtr &robot)
{
    if (!dirty)
        return;

    frames->dirty();
    robot->fk(data, frames);

    dirty = false;
}

const Eigen::Isometry2d &State::getPose(const RobotPtr &robot, const std::string &frame)
{
    if (dirty)
        fk(robot);

    return frames->getEntryConst(frame).pose;
}

const Eigen::MatrixXd &State::getJacobian(const RobotPtr &robot, const std::string &frame)
{
    return robot->getJacobian(frame, data, frames).jacobian;
}

const tf::EigenVector<Eigen::Isometry2d> &State::getGeometryPoses(const RobotPtr &robot,
                                                                  const std::string &frame)
{
    if (dirty)
        fk(robot);

    return frames->getEntryConst(frame).geometry;
}

std::string State::printFrames(const RobotPtr &robot) const
{
    return frames->printFrames(robot);
}