NJointTrajectoryController.cpp

Go to the documentation of this file.

#include "NJointTrajectoryController.h"
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
namespace armarx
{
    NJointControllerRegistration<NJointTrajectoryController>
        registrationControllerNJointTrajectoryController("NJointTrajectoryController");
 
    NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov,
                                                           const NJointControllerConfigPtr& config,
                                                           const VirtualRobot::RobotPtr& robot)
    {
        cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointTrajectoryControllerConfigPtr";
 
        for (std::string jointName : cfg->jointNames)
        {
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
            sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
        }
        if (cfg->jointNames.size() == 0)
        {
            ARMARX_ERROR << "cfg->jointNames.size() == 0";
        }
        for (auto& jointName : cfg->jointNames)
        {
            LimitlessState ls;
            ls.enabled = false;
            VirtualRobot::RobotNodePtr rn = robot->getRobotNode(jointName);
            if (rn)
            {
                ls.enabled = rn->isLimitless();
                ls.limitLo = rn->getJointLimitLo();
                ls.limitHi = rn->getJointLimitHi();
            }
            ARMARX_DEBUG << "limitless status - " << jointName << ": " << rn->isLimitless();
            limitlessStates.push_back(ls);
        }
    }
 
    NJointTrajectoryController::~NJointTrajectoryController()
    {
    }
 
    std::string
    NJointTrajectoryController::getClassName(const Ice::Current&) const
    {
        return "NJointTrajectoryController";
    }
 
    void
    NJointTrajectoryController::onInitNJointController()
    {
        offeringTopic("StaticPlotter");
    }
 
    void
    NJointTrajectoryController::onConnectNJointController()
    {
        plotter = getTopic<StaticPlotterInterfacePrx>("StaticPlotter");
        dbgObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
    }
 
    void
    NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                      const IceUtil::Time& timeSinceLastIteration)
    {
        if (rtGetControlStruct().trajectoryCtrl)
        {
            TrajectoryController& contr = *rtGetControlStruct().trajectoryCtrl;
            ARMARX_CHECK_EQUAL(sensors.size(), targets.size());
            const auto& dimNames = contr.getTraj()->getDimensionNames();
            for (size_t i = 0; i < sensors.size(); i++)
            //            for(auto& s : sensors)
            {
                ARMARX_CHECK_EQUAL(dimNames.at(i), cfg->jointNames.at(i));
                currentPos(i) = sensors.at(i)->position;
            }
 
            auto& newVelocities =
                contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
            //            StringVariantBaseMap positions;
            //            ARMARX_RT_LOGF_INFO("Current error %.3f",  contr.getCurrentError()(1)).deactivateSpam(0.1);
 
            //            for (int i = 0; i < contr.getCurrentError().rows(); ++i)
            //            {
            //                positions[dimNames.at(i) + "-targetPosition"] = new Variant(contr.getPositions()(i));
            //                positions[dimNames.at(i) + "-currentPosition"] = new Variant(currentPos(i));
            //            }
            //            dbgObs->setDebugChannel("positions", positions);
            currentTimestamp = contr.getCurrentTimestamp();
            finished =
                (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() &&
                 direction == 1.0) ||
                (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
 
            for (size_t i = 0; i < targets.size(); ++i)
            {
                {
                    targets.at(i)->velocity =
                        (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
                }
            }
 
            if (finished && looping)
            {
                direction *= -1.0;
            }
        }
    }
 
    void
    NJointTrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
    {
        TIMING_START(TrajectoryOptimization);
        ARMARX_CHECK_EXPRESSION(t);
        TrajectoryPtr trajectory = TrajectoryPtr::dynamicCast(t);
        ARMARX_CHECK_EXPRESSION(trajectory);
        size_t trajectoryDimensions = trajectory->dim();
        ARMARX_CHECK_EQUAL(trajectoryDimensions, targets.size());
        trajectory->setLimitless(limitlessStates);
        //        trajectory->gaussianFilter(0.3);
        if (cfg->considerConstraints)
        {
            std::list<Eigen::VectorXd> pathPoints;
            double timeStep = cfg->preSamplingStepMs / 1000.0;
            Eigen::VectorXd maxVelocities =
                Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
            Eigen::VectorXd maxAccelerations =
                Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
            TrajectoryController::UnfoldLimitlessJointPositions(trajectory);
            for (const Trajectory::TrajData& element : *trajectory)
            {
                Eigen::VectorXd waypoint(trajectoryDimensions);
                for (size_t i = 0; i < trajectoryDimensions; ++i)
                {
                    waypoint(i) = element.getPosition(i);
                }
                pathPoints.emplace_back(waypoint);
            }
 
            VirtualRobot::Path p(pathPoints, cfg->maxDeviation);
            VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(
                p, maxVelocities, maxAccelerations, timeStep);
 
            TrajectoryPtr newTraj = new Trajectory();
 
            Ice::DoubleSeq newTimestamps;
            double duration = timeOptimalTraj.getDuration();
            for (double t = 0.0; t < duration; t += timeStep)
            {
                newTimestamps.push_back(t);
            }
            newTimestamps.push_back(duration);
 
            for (size_t d = 0; d < trajectoryDimensions; d++)
            {
                //            Ice::DoubleSeq position;
                //            for (double t = 0.0; t < duration; t += timeStep)
                //            {
                //                position.push_back(timeOptimalTraj.getPosition(t)[d]);
                //            }
                //            position.push_back(timeOptimalTraj.getPosition(duration)[d]);
                //            newTraj->addDimension(position, newTimestamps, trajectory->getDimensionName(d));
 
                Ice::DoubleSeq derivs;
                for (double t = 0.0; t < duration; t += timeStep)
                {
                    derivs.clear();
                    derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
                    derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
                    newTraj->addDerivationsToDimension(d, t, derivs);
                }
                derivs.clear();
                derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
                derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
                newTraj->addDerivationsToDimension(d, duration, derivs);
            }
            newTraj->setDimensionNames(trajectory->getDimensionNames());
            newTraj->setLimitless(limitlessStates);
            TrajectoryController::FoldLimitlessJointPositions(newTraj);
            TIMING_END(TrajectoryOptimization);
            ARMARX_INFO << "Trajectory duration: " << newTraj->getTimeLength();
            ARMARX_INFO << VAROUT(newTraj->output());
            trajectory = newTraj;
        }
 
        trajEndTime = *trajectory->getTimestamps().rbegin() - *trajectory->getTimestamps().begin();
        currentPos.resize(trajectory->getDimensionNames().size());
 
        startTimestamp = *trajectory->getTimestamps().begin();
        endTimestamp = *trajectory->getTimestamps().rbegin();
        if (plotter)
        {
            StringFloatSeqDict posData;
            StringFloatSeqDict velData;
            for (size_t d = 0; d < trajectory->dim(); ++d)
            {
                auto positions = trajectory->getDimensionData(d, 0);
                posData[trajectory->getDimensionName(d)] =
                    Ice::FloatSeq(positions.begin(), positions.end());
                auto velocities = trajectory->getDimensionData(d, 1);
                velData[trajectory->getDimensionName(d)] =
                    Ice::FloatSeq(velocities.begin(), velocities.end());
            }
            auto timestampsDouble = trajectory->getTimestamps();
            Ice::FloatSeq timestamps(timestampsDouble.begin(), timestampsDouble.end());
            plotter->addPlotWithTimestampVector("Positions", timestamps, posData);
            plotter->addPlotWithTimestampVector("Velocities", timestamps, velData);
        }
        else
        {
            ARMARX_WARNING << "Plotter proxy is NULL";
        }
 
 
        LockGuardType guard{controlDataMutex};
        ARMARX_INFO << VAROUT(cfg->PID_p) << VAROUT(cfg->PID_i) << VAROUT(cfg->PID_d);
 
        trajectoryCtrl.reset(
            new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
        getWriterControlStruct().trajectoryCtrl = trajectoryCtrl;
        writeControlStruct();
    }
 
    bool
    NJointTrajectoryController::isFinished(const Ice::Current&)
    {
        return finished;
    }
 
    double
    NJointTrajectoryController::getCurrentTimestamp(const Ice::Current&)
    {
        return currentTimestamp;
    }
 
    float
    NJointTrajectoryController::getCurrentProgressFraction(const Ice::Current&)
    {
        return math::MathUtils::ILerp(startTimestamp, endTimestamp, currentTimestamp);
    }
 
    double
    NJointTrajectoryController::getCurrentTrajTime() const
    {
        LockGuardType guard{controlDataMutex};
        if (trajectoryCtrl)
        {
            return trajectoryCtrl->getCurrentTimestamp();
        }
        else
        {
            return 0.0;
        }
    }
 
    void
    NJointTrajectoryController::setLooping(bool looping)
    {
        this->looping = looping;
    }
 
    double
    NJointTrajectoryController::getTrajEndTime() const
    {
        return trajEndTime;
    }
 
    TrajectoryPtr
    NJointTrajectoryController::getTrajectoryCopy() const
    {
        LockGuardType guard{controlDataMutex};
        if (trajectoryCtrl)
        {
            return TrajectoryPtr::dynamicCast(trajectoryCtrl->getTraj()->clone());
        }
        else
        {
            return new Trajectory();
        }
    }
 
    void
    NJointTrajectoryController::rtPreActivateController()
    {
        startTime = IceUtil::Time();
    }
 
    void
    NJointTrajectoryController::rtPostDeactivateController()
    {
        for (auto& target : targets)
        {
            target->velocity = 0.0f;
        }
    }
 
} // namespace armarx