TrajectoryControllerSubUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::TrajectoryControllerSubUnit
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TrajectoryControllerSubUnit.h"
 
using namespace armarx;
 
void
TrajectoryControllerSubUnit::onInitComponent()
{
    kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
    usingProxy(kinematicUnitName);
    robotPoseUnitEnabled = getProperty<bool>("EnableRobotPoseUnit").getValue();
    robotPoseUnitName = getProperty<std::string>("RobotPoseUnitName").getValue();
 
    kp = getProperty<float>("Kp").getValue();
    ki = getProperty<float>("Ki").getValue();
    kd = getProperty<float>("Kd").getValue();
    maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 *
                  M_PI; // config expects value in rad/sec
 
    offeringTopic("DebugDrawerUpdates");
}
 
void
TrajectoryControllerSubUnit::onConnectComponent()
{
    kinematicUnit = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
    debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
}
 
void
TrajectoryControllerSubUnit::onDisconnectComponent()
{
    debugDrawer->clearLayer("Preview");
}
 
bool
TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
{
    ARMARX_DEBUG << "Starting TrajectoryPlayer ...";
 
    ARMARX_CHECK_EXPRESSION(jointTraj) << "No trajectory loaded!";
    assureTrajectoryController();
 
    jointTrajController->setTrajectory(this->jointTraj, c);
    jointTraj = jointTrajController->getTrajectoryCopy();
    jointTrajController->activateController();
 
    if (isPreview)
    {
        std::string fileName = kinematicUnit->getRobotFilename();
        ARMARX_INFO << "robot file name : " << fileName;
        debugDrawer->setRobotVisu("Preview",
                                  "previewRobot",
                                  fileName,
                                  fileName.substr(0, fileName.find("/")),
                                  armarx::CollisionModel);
        debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor{0, 1, 0, 0.5});
 
        previewTask =
            new PeriodicTask<TrajectoryControllerSubUnit>(this,
                                                          &TrajectoryControllerSubUnit::previewRun,
                                                          20,
                                                          false,
                                                          "TrajectoryControllerSubUnitPreviewTask",
                                                          false);
        previewTask->start();
    }
 
    return true;
}
 
bool
TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
{
    if (controllerExists())
    {
        if (jointTrajController->isControllerActive())
        {
            ARMARX_DEBUG << "Pausing TrajectoryPlayer ...";
            jointTrajController->deactivateController();
        }
        else
        {
            ARMARX_DEBUG << "Resuming TrajectoryPlayer ...";
            jointTrajController->activateController();
        }
    }
    return true;
}
 
bool
TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
{
    ARMARX_DEBUG << "Stopping TrajectoryPlayer ...";
 
    if (controllerExists())
    {
        if (isPreview)
        {
            previewTask->stop();
            debugDrawer->clearLayer("Preview");
        }
 
        jointTrajController->deactivateController();
        while (jointTrajController->isControllerActive())
        {
        }
        jointTrajController->deleteController();
    }
    return true;
}
 
bool
TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& c)
{
    ARMARX_DEBUG << "Resetting TrajectoryPlayer ...";
 
    if (controllerExists() && jointTrajController->isControllerActive())
    {
        jointTrajController->deactivateController();
        while (jointTrajController->isControllerActive())
        {
        }
    }
 
    if (!jointTraj)
    {
        ARMARX_INFO << "No trajectory loaded! Cannot reset to FrameZeroPose!";
        return false;
    }
 
    assureTrajectoryController();
 
    jointTrajController->setTrajectory(this->jointTraj, c);
    if (moveToFrameZeroPose)
    {
        std::vector<Ice::DoubleSeq> states = jointTraj->getAllStates(0.0f, 1);
        NameValueMap frameZeroPositions;
        NameControlModeMap controlModes;
 
        auto dimNames = jointTraj->getDimensionNames();
        for (size_t i = 0; i < dimNames.size(); i++)
        {
            const auto& jointName = dimNames.at(i);
            if (std::find(usedJoints.begin(), usedJoints.end(), jointName) != usedJoints.end())
            {
                frameZeroPositions[jointName] = states[i][0];
                controlModes[jointName] = ePositionControl;
            }
        }
 
        kinematicUnit->switchControlMode(controlModes);
        kinematicUnit->setJointAngles(frameZeroPositions);
    }
    return true;
}
 
void
TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTraj,
                                           const Ice::Current& c)
{
    ARMARX_DEBUG << "Loading Trajectory ...";
 
    std::unique_lock lock(dataMutex);
 
    this->jointTraj = TrajectoryPtr::dynamicCast(jointTraj);
 
    if (!this->jointTraj)
    {
        ARMARX_ERROR << "Error when loading TrajectoryPlayer: cannot load jointTraj !!!";
        return;
    }
 
    if (this->jointTraj->size() == 0)
    {
        ARMARX_ERROR << "Error when loading TrajectoryPlayer: trajectory is empty !!!";
        return;
    }
    ARMARX_INFO << VAROUT(this->jointTraj->getDimensionNames());
 
    auto startTime = this->jointTraj->begin()->getTimestamp();
    this->jointTraj->shiftTime(-startTime);
    bool differentJointSet = usedJoints.size() != this->jointTraj->getDimensionNames().size();
    if (!differentJointSet)
    {
        for (size_t i = 0; i < usedJoints.size(); i++)
        {
            if (usedJoints.at(i) != this->jointTraj->getDimensionNames().at(i))
            {
                differentJointSet = true;
                break;
            }
        }
    }
    usedJoints = this->jointTraj->getDimensionNames();
    ARMARX_INFO << VAROUT(usedJoints);
 
    if (usedJoints.size() != this->jointTraj->dim())
    {
        ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when "
                        "using kinematicUnit)";
        return;
    }
 
    if (!jointTrajController || differentJointSet || recreateController)
    {
        jointTrajController = createTrajectoryController(usedJoints, true);
    }
    jointTrajController->setTrajectory(this->jointTraj, c);
 
    endTime = jointTrajController->getTrajEndTime();
    trajEndTime = endTime;
}
 
void
TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj,
                                              const Ice::Current& c)
{
    this->basePoseTraj = TrajectoryPtr::dynamicCast(basePoseTraj);
}
 
void
TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
{
    std::unique_lock lock(dataMutex);
 
    if (!controllerExists())
    {
        ARMARX_WARNING << "No trajectory has been loaded and therefore no controller exists.";
        return;
    }
    jointTrajController->setLooping(loop);
}
 
double
TrajectoryControllerSubUnit::getEndTime(const Ice::Current& c)
{
    return endTime;
}
 
double
TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
{
    return trajEndTime;
}
 
double
TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
{
    std::unique_lock lock(dataMutex);
 
    if (!controllerExists())
    {
        ARMARX_WARNING << "No trajectory has been loaded and therefore no controller exists.";
        return 0.0;
    }
    return jointTrajController->getCurrentTrajTime();
}
 
void
TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current& c)
{
    ARMARX_DEBUG << "Setting end-time ...";
 
    std::unique_lock lock(dataMutex);
 
    if (!jointTraj)
    {
        ARMARX_WARNING << "No trajectory has been loaded!";
        return;
    }
    assureTrajectoryController();
 
    if (jointTrajController->isControllerActive())
    {
        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting a "
                          "new end-time.";
        return;
    }
 
    if (endTime == end)
    {
        return;
    }
 
    bool b = considerConstraintsForTrajectoryOptimization;
    considerConstraintsForTrajectoryOptimization = false;
 
    endTime = end;
    jointTraj = jointTraj->normalize(0, endTime);
    if (basePoseTraj)
    {
        basePoseTraj = basePoseTraj->normalize(0, endTime);
    }
 
    jointTrajController->setTrajectory(jointTraj, c);
 
    considerConstraintsForTrajectoryOptimization = b;
}
 
void
TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
{
    if (controllerExists() && jointTrajController->isControllerActive())
    {
        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
                          "whether the controller is only for preview.";
        return;
    }
    this->isPreview = isPreview;
 
    if (jointTraj)
    {
        jointTrajController = createTrajectoryController(usedJoints, true);
        jointTrajController->setTrajectory(jointTraj, c);
    }
}
 
bool
TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName,
                                            bool isInUse,
                                            const Ice::Current& c)
{
    ARMARX_DEBUG << "Setting joints in use ...";
 
    std::unique_lock lock(dataMutex);
 
    if (controllerExists())
    {
        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
                          "joints in use.";
    }
    else
    {
        if (isInUse &&
            (std::find(usedJoints.begin(), usedJoints.end(), jointName) == usedJoints.end()))
        {
            usedJoints.push_back(jointName);
        }
        else if (!isInUse)
        {
            auto it = std::find(usedJoints.begin(), usedJoints.end(), jointName);
            if (it != usedJoints.end())
            {
                std::swap(*it, usedJoints.back());
                usedJoints.pop_back();
            }
        }
 
        if (jointTraj)
        {
            jointTrajController = createTrajectoryController(usedJoints, true);
            jointTrajController->setTrajectory(jointTraj, c);
        }
    }
 
    return isInUse;
}
 
void
TrajectoryControllerSubUnit::enableRobotPoseUnit(bool isRobotPose, const Ice::Current& c)
{
    ARMARX_WARNING << "NYI : TrajectoryControllerSubUnit::enableRobotPoseUnit()";
}
 
void
TrajectoryControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
{
}
 
void
TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::Current& c)
{
    if (controllerExists() && jointTrajController->isControllerActive())
    {
        ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
                          "whether constraints should be considered.";
        return;
    }
    considerConstraintsForTrajectoryOptimization = consider;
 
    if (jointTraj)
    {
        jointTrajController = createTrajectoryController(usedJoints, false);
        jointTrajController->setTrajectory(jointTraj, c);
    }
}
 
NJointTrajectoryControllerPtr
TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames,
                                                        bool deleteIfAlreadyExist)
{
    std::unique_lock lock(controllerMutex);
 
    if (controllerExists() && deleteIfAlreadyExist)
    {
        jointTrajController->deactivateController();
        while (jointTrajController->isControllerActive())
        {
            usleep(500);
        }
        jointTrajController->deleteController();
        while (getArmarXManager()->getIceManager()->isObjectReachable(controllerName))
        {
            usleep(500);
        }
    }
 
    NJointTrajectoryControllerPtr trajController = jointTrajController;
    if (!controllerExists() || deleteIfAlreadyExist)
    {
        recreateController = false;
        NJointTrajectoryControllerConfigPtr config = new NJointTrajectoryControllerConfig();
        ARMARX_INFO << VAROUT(kp) << VAROUT(ki) << VAROUT(kd);
        config->PID_p = kp;
        config->PID_i = ki;
        config->PID_d = kd;
        config->maxVelocity = maxVelocity;
        config->jointNames = jointNames;
        config->considerConstraints = considerConstraintsForTrajectoryOptimization;
        config->isPreview = isPreview;
        controllerName = this->getName() + "_" + "JointTrajectory" + IceUtil::generateUUID();
        trajController =
            NJointTrajectoryControllerPtr::dynamicCast(robotUnit->createNJointController(
                "NJointTrajectoryController", controllerName, config, true, true));
 
        while (!getArmarXManager()->getIceManager()->isObjectReachable(controllerName))
        {
            usleep(500);
        }
    }
    return trajController;
}
 
void
TrajectoryControllerSubUnit::assureTrajectoryController()
{
    std::unique_lock lock(controllerMutex);
 
    if (!controllerExists())
    {
        jointTrajController = createTrajectoryController(usedJoints, false);
    }
    ARMARX_CHECK_EXPRESSION(jointTrajController);
}
 
bool
TrajectoryControllerSubUnit::controllerExists()
{
    std::unique_lock lock(controllerMutex);
 
    auto allNJointControllers =
        robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
    NJointTrajectoryControllerPtr trajController;
    for (const auto& controller : allNJointControllers)
    {
        trajController = NJointTrajectoryControllerPtr::dynamicCast(controller);
        if (!trajController)
        {
            continue;
        }
        if (trajController->getName() == controllerName)
        {
            jointTrajController = trajController;
            return true;
        }
    }
    return false;
}
 
void
TrajectoryControllerSubUnit::previewRun()
{
    if (controllerExists())
    {
        if (jointTrajController->isControllerActive())
        {
            std::vector<Ice::DoubleSeq> states =
                jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
            NameValueMap targetPositionValues;
 
            auto dimNames = jointTraj->getDimensionNames();
            for (size_t i = 0; i < dimNames.size(); i++)
            {
                const auto& jointName = dimNames.at(i);
                if (std::find(usedJoints.begin(), usedJoints.end(), jointName) != usedJoints.end())
                {
                    targetPositionValues[jointName] = states[i][0];
                }
            }
            debugDrawer->updateRobotConfig("Preview", "previewRobot", targetPositionValues);
        }
    }
}
 
void
TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
{
    ARMARX_CHECK_EXPRESSION(!robotUnit);
    ARMARX_CHECK_EXPRESSION(rUnit);
    robotUnit = rUnit;
}
 
void
armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(
    const std::set<std::string>& changedProperties)
{
    ARMARX_INFO << "Changning properties";
    if (changedProperties.count("Kp"))
    {
        ARMARX_INFO << "Changning property Kp";
        kp = getProperty<float>("Kp").getValue();
        recreateController = true;
    }
 
    if (changedProperties.count("Kd"))
    {
        ARMARX_INFO << "Changning property Kd";
        kd = getProperty<float>("Kd").getValue();
        recreateController = true;
    }
 
    if (changedProperties.count("Ki"))
    {
        ARMARX_INFO << "Changning property Ki";
        ki = getProperty<float>("Ki").getValue();
        recreateController = true;
    }
}