Cartesian_HybCntrl.cpp

Go to the documentation of this file.

#include "robodyn_controllers/Cartesian_HybCntrl.h"

Cartesian_HybCntrl::Cartesian_HybCntrl()
    : logCategory("gov.nasa.controllers.CartHybCntrl")
{
    linearRateLimiter.reset(new RateLimiter);
    angularRateLimiter.reset(new RateLimiter);
}

Cartesian_HybCntrl::~Cartesian_HybCntrl()
{
}

void Cartesian_HybCntrl::setGain(double forceP, double torqueP, double forceI, double torqueI, double forceD, double torqueD, double forceWindup, double torqueWindup, double positionErrorLimit_in, double rotationErrorLimit_in)
{
    fKp = forceP;
    tKp = torqueP;
    fKi = forceI;
    tKi = torqueI;
    fKd = forceD;
    tKd = torqueD;
    forceIntegralWindupLimit = forceWindup;
    torqueIntegralWindupLimit = torqueWindup;
    positionErrorLimit = positionErrorLimit_in;
    rotationErrorLimit = rotationErrorLimit_in;

    edgeBuffer = 0.25; // percentage of the edge to buffer the gains

    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "set force and torque error gain";
}

void Cartesian_HybCntrl::setPoseSettings(double linVel, double angVel, double linAcc, double angAcc)
{
    maxLinearVelocity = linVel;
    maxAngularVelocity = angVel;

    linearRateLimiter->setRateLimit(linAcc);
    angularRateLimiter->setRateLimit(angAcc);

    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "set pose settings for maxLinear, maxAngular Velocity and RateLimiter";
}

void Cartesian_HybCntrl::setFilter(double filterAlpha)
{
    alpha = filterAlpha;
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "set filter alpha";
}

void Cartesian_HybCntrl::setSensorNameMap(const std::map<std::string, std::string>& sensorNameMap)
{
    this->sensorNameMap = sensorNameMap;
}

void Cartesian_HybCntrl::updateSensorForces(const std::map<std::string, KDL::Wrench>& sensorForces)
{
    for (sensorForceItr = sensorForces.begin(); sensorForceItr != sensorForces.end(); ++sensorForceItr)
    {
        if (sensorForceMap.find(sensorForceItr->first) == sensorForceMap.end())
        {
            sensorForceMap[sensorForceItr->first] = KDL::Wrench::Zero();
        }

        sensorForceMap[sensorForceItr->first] = alpha * sensorForceMap[sensorForceItr->first] + (1 - alpha) * sensorForceItr->second;
    }
}

bool Cartesian_HybCntrl::setInitialPose(const std::string& frameName, const::KDL::Frame& pose)
{
    if (forceCntrlMap.find(frameName) == forceCntrlMap.end())
    {
        NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.core.CartHybCntrl") << log4cpp::Priority::ERROR << frameName << " not in force control";
        return false;
    }
    cout << "Cartetian_HybCntrl::reset for initial pose" <<endl;
    printFrame("Reset_Pose", pose);
    forceCntrlMap.at(frameName).pose = pose;
    return true;
}

void Cartesian_HybCntrl::createForceControlMap(std::map<std::string, std::pair<std::vector<int>, std::vector<double> > > forceNodes)
{
    forceCntrlMap.clear();

    for (std::map<std::string, std::pair<std::vector<int>, std::vector<double> > >::iterator itr = forceNodes.begin(); itr != forceNodes.end(); ++itr)
    {
        if (itr->second.first.size() != itr->second.second.size())
        {
            NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << "force axes and magnitudes do not match for node " << itr->first << "!";
            continue;
        }

        ForceControl newForceControl;
        newForceControl.sensorName = getSensorNameFromNode(itr->first);

        for (unsigned int i = 0; i < itr->second.first.size(); ++i)
        {
            switch (itr->second.first.at(i))
            {
                case r2_msgs::ForceControlAxis::X:
                    newForceControl.x = true;
                    newForceControl.desiredForce.force.x(itr->second.second.at(i));
                    break;

                case r2_msgs::ForceControlAxis::Y:
                    newForceControl.y = true;
                    newForceControl.desiredForce.force.y(itr->second.second.at(i));
                    break;

                case r2_msgs::ForceControlAxis::Z:
                    newForceControl.z = true;
                    newForceControl.desiredForce.force.z(itr->second.second.at(i));
                    break;

                case r2_msgs::ForceControlAxis::ROLL:
                    newForceControl.roll = true;
                    newForceControl.desiredForce.torque.x(itr->second.second.at(i));
                    break;

                case r2_msgs::ForceControlAxis::PITCH:
                    newForceControl.pitch = true;
                    newForceControl.desiredForce.torque.y(itr->second.second.at(i));
                    break;

                case r2_msgs::ForceControlAxis::YAW:
                    newForceControl.yaw = true;
                    newForceControl.desiredForce.torque.z(itr->second.second.at(i));
                    break;

                default:
                    break;
            }
        }

        forceCntrlMap[itr->first] = newForceControl;
    }
}

void Cartesian_HybCntrl::printFrame(std::string name, KDL::Frame frame)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << name << ": "
        << frame.p.x() << ", " << frame.p.y() << ", " << frame.p.z() << ", "
        << frame.M.GetRot().x() << ", " << frame.M.GetRot().y() << ", " << frame.M.GetRot().z();

    double alpha = 0;
    double beta = 0;
    double gamma = 0;
    frame.M.GetEulerZYX(alpha, beta, gamma);

    cout << "frame {" << name <<
        "} position = (" <<
        frame.p.x() << ", " <<
        frame.p.y() << ", " <<
        frame.p.z() << ") " <<
        "orientaion = (" <<
        gamma << ", " <<
        beta << ", " <<
        alpha << ")" << endl;
}

void Cartesian_HybCntrl::printWrench(std::string name, KDL::Wrench wrench)
{
  cout <<
      "{" << name << "} " <<
      "Force = (" <<
      wrench.force.x() << ", " <<
      wrench.force.y() << ", " <<
      wrench.force.z() << ") " <<
      "Torque = (" <<
      wrench.torque.x() << ", " <<
      wrench.torque.y() << ", " <<
      wrench.torque.z() << ")" << endl;
}


void Cartesian_HybCntrl::updateFrames(std::vector<std::string> nodeNames, std::map<std::string, KDL::Frame> referenceFrameMap, std::vector<KDL::Frame>& nodeFrames, double dt)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "updating frames..";

    if (nodeNames.size() != nodeFrames.size())
    {
        std::stringstream err;
        err << "nodeNames and nodeFrames must be the same size! nodeNames: " << nodeNames.size() << ", nodeFrames: " << nodeFrames.size();
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    for (forceItr = forceCntrlMap.begin(); forceItr != forceCntrlMap.end(); ++forceItr)
    {
        sensorForceItr = sensorForceMap.find(forceItr->second.sensorName);

        if (sensorForceItr == sensorForceMap.end())
        {
            std::stringstream err;
            err << "Unable to find actual force for " << forceItr->second.sensorName;
            NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << err.str();
            throw std::runtime_error(err.str());
        }

        forceItr->second.sensorForce = sensorForceItr->second;
    }

    for (unsigned int i = 0; i < nodeNames.size(); ++i)
    {
        std::stringstream ss;

        if ((forceItr = forceCntrlMap.find(nodeNames.at(i))) != forceCntrlMap.end())
        {
            //cout << "force gain = " << fKp <<endl;
            try
            {
                poseFrame = referenceFrameMap.at(forceItr->first);
                //printFrame("Pose_Frame", poseFrame);
            }
            catch (std::exception& e)
            {
                std::stringstream err;
                err << "Could not find pose frame " << forceItr->first << " in referenceFrameMap!";
                NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
                throw std::runtime_error(err.str());
            }

            try
            {
                sensorFrame = referenceFrameMap.at(forceItr->second.sensorName);
                //printFrame("sensor_frame", sensorFrame);
            }
            catch (std::exception& e)
            {
                std::stringstream err;
                err << "Could not find sensor frame " << forceItr->second.sensorName << " in referenceFrameMap!";
                NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
                throw std::runtime_error(err.str());
            }

            // rename the desired pose to do operations
            desiredPose = nodeFrames.at(i);

            //print the previous frame
            //printFrame("previous_frame", forceItr->second.pose);

            // Get the current sensor wrench
            wrenchSensorFrame = forceItr->second.sensorForce;
            //printWrench("wrenchSensorFrame", wrenchSensorFrame);

            //get the desired force
            wrenchDesired = forceItr->second.desiredForce;
            //printWrench("wrenchDesired", wrenchDesired);

            // convert the wrench from the sensor frame to the pose frame
            KDL::Frame T_FP;  // Transformation from Force to Pose
            T_FP = poseFrame.Inverse() * sensorFrame;

            // according the KDL docs, multiplying a Wrench by the transformation
            // from A to B will actually do the Force transformation, which is not just
            // a normal transformation matrix. See Craig book pg 158
            wrenchPoseFrame = T_FP * wrenchSensorFrame;
            //printWrench("wrenchPoseFrame", wrenchPoseFrame);

            // Calculate the force error
            forceError = wrenchDesired - wrenchPoseFrame; // do calcs in the pose frame
            //printWrench("forceError", forceError);

            // calculate the PID gains in the pose frame

            //Derivative Gain
            forceItr->second.derivative = (forceError - forceItr->second.error) / dt;
            forceItr->second.derivative.force = fKd * forceItr->second.derivative.force;
            forceItr->second.derivative.torque = tKd * forceItr->second.derivative.torque;
            //printWrench("derivatie_gain", forceItr->second.derivative);

            //Integral Gain
            //printWrench("pre_integrator_wrench", forceItr->second.integrator);
            forceItr->second.integrator.force += fKi * forceError.force;
            forceItr->second.integrator.torque += tKi * forceError.torque;
            //printWrench("integrator_gain_pre_limi", forceItr->second.integrator);
            limitVector(-forceIntegralWindupLimit, forceIntegralWindupLimit, forceItr->second.integrator.force);
            limitVector(-torqueIntegralWindupLimit, torqueIntegralWindupLimit, forceItr->second.integrator.torque);
            //printWrench("integrator_gain_post_limit", forceItr->second.integrator);


            //Proportional Gain
            forceItr->second.error = forceError; // this makes stores it for us
            forceItr->second.gain.force = fKp * forceError.force;
            forceItr->second.gain.torque = tKp * forceError.torque;
            //printWrench("proportional_gain", forceItr->second.gain);

            // forcePID in the pose_frame
            forcePID = forceItr->second.gain + forceItr->second.integrator + forceItr->second.derivative;
            printWrench("forcePID", forcePID);


            // Convert the desired pose into the pose frame and previous frame for calculations
            desiredPose = poseFrame.Inverse() * desiredPose;
            forceItr->second.pose = poseFrame.Inverse() * forceItr->second.pose;
            printFrame("converted_desiredPose", desiredPose);

            if (doFeedForward)
            {
                velocity = KDL::Twist(forcePID.force, forcePID.torque) + forceItr->second.prevVelocity;
            }
            else
            {
                velocity = KDL::Twist(forcePID.force, forcePID.torque);
            }

            limitVelocity(forceItr->second.prevVelocity, velocity);

            //TODO: This will only add position to the xyz, not roll,pitch,yaw right now
            KDL::Frame new_desiredPose = desiredPose;
            if (forceItr->second.x)
            {
                new_desiredPose.p.x(forceItr->second.pose.p.x() + velocity.vel.x() * dt);
                velocity.vel.x(edgeLimit(velocity.vel.x(), new_desiredPose.p.x(), desiredPose.p.x()));
                desiredPose.p.x(forceItr->second.pose.p.x() + velocity.vel.x() * dt);
            }
            if (forceItr->second.y)
            {
                new_desiredPose.p.y(forceItr->second.pose.p.y() + velocity.vel.y() * dt);
                velocity.vel.y(edgeLimit(velocity.vel.y(), new_desiredPose.p.y(), desiredPose.p.y()));
                desiredPose.p.y(forceItr->second.pose.p.y() + velocity.vel.y() * dt);
            }
            if (forceItr->second.z)
            {
                new_desiredPose.p.z(forceItr->second.pose.p.z() + velocity.vel.z() * dt);
                velocity.vel.z(edgeLimit(velocity.vel.z(), new_desiredPose.p.z(), desiredPose.p.z()));
                desiredPose.p.z( forceItr->second.pose.p.z() + velocity.vel.z() * dt) ;
            }
            if (forceItr->second.roll)
            {
                desiredPose.M.DoRotX( velocity.rot.x() * dt );
            }
            if (forceItr->second.pitch)
            {
                desiredPose.M.DoRotY( velocity.rot.y() * dt );
            }
            if (forceItr->second.yaw)
            {
                desiredPose.M.DoRotZ( velocity.rot.z() * dt );
            }
            //printFrame("New_desiredPose", desiredPose);

            //convert back to world frame
            desiredPose = poseFrame * desiredPose;
            forceItr->second.pose = poseFrame * forceItr->second.pose;

            // limit the pose error
            limitPoseError(desiredPose, nodeFrames.at(i));
            //printFrame("FINAL_desiredPose", desiredPose);


            nodeFrames[i] = desiredPose;
            //printFrame("Controller_Node_Frames_Out", nodeFrames[i]);
            forceItr->second.pose = desiredPose;
            forceItr->second.prevVelocity = velocity;
        }
    }
}

double Cartesian_HybCntrl::edgeLimit(double vel, double prev, double input)
{
  // Take in the current velociy (aka, the forcePID value) and check to make sure
  // it is not with 25% of the error bound positionErrorLimit. If so, limit the PID
  // velocity value to less a the percentage and set that velocity.
  double comp = prev - input;
  if (comp > ((1-edgeBuffer) * positionErrorLimit) || comp <  -((1-edgeBuffer) * positionErrorLimit))
  {
      cout << "limiting vel from "<< vel << " to";
      vel = vel * (1 - (abs(comp) - (1-edgeBuffer) * positionErrorLimit) / (edgeBuffer * positionErrorLimit));
      cout << vel << endl;
  }
  return vel;
}

void Cartesian_HybCntrl::limitPoseError(KDL::Frame &output_pose, KDL::Frame input_pose)
{
  // This function takes the current output pose and limits it based on the prescribed
  // maximum distance from the input pose
  //TODO: Make this work for rotations as well.

  if (output_pose.p.x() - input_pose.p.x() > positionErrorLimit)
  {
      output_pose.p.x(input_pose.p.x() + positionErrorLimit);
      cout<<"above x limit"<<endl;
  }
  else if (output_pose.p.x() - input_pose.p.x() < -positionErrorLimit)
  {
      output_pose.p.x(input_pose.p.x() - positionErrorLimit);
      cout<<"below x limit"<<endl;
  }

  // Y
  if (output_pose.p.y() - input_pose.p.y() > positionErrorLimit)
  {
      output_pose.p.y(input_pose.p.y() + positionErrorLimit);
      cout<<"above y limit"<<endl;
  }
  else if (output_pose.p.y() - input_pose.p.y() < -positionErrorLimit)
  {
      output_pose.p.y(input_pose.p.y() - positionErrorLimit);
      cout<<"below y limit"<<endl;
  }

  // Z
  if (output_pose.p.z() - input_pose.p.z() > positionErrorLimit)
  {
      output_pose.p.z(input_pose.p.z() + positionErrorLimit);
      cout<<"above z limit"<<endl;
  }
  else if (output_pose.p.z() - input_pose.p.z() < -positionErrorLimit)
  {
      output_pose.p.z(input_pose.p.z() - positionErrorLimit);
      cout<<"below z limit"<<endl;
  }
}

void Cartesian_HybCntrl::limitVector(double minVal, double maxVal, KDL::Vector& vector)
{
    int i = 0;

    for (; i < 3; ++i)
    {
        Robodyn::limit(minVal, maxVal, vector[i]);
    }
}

void Cartesian_HybCntrl::limitVelocity(const KDL::Twist& pastVelocity, KDL::Twist& currVelocity)
{
    for (int i = 0; i < 3; ++i)
    {
        currVelocity.vel(i) = linearRateLimiter->getLimitedValue(currVelocity.vel(i), pastVelocity.vel(i));
        currVelocity.rot(i) = angularRateLimiter->getLimitedValue(currVelocity.rot(i), pastVelocity.rot(i));
        limitVector(-maxLinearVelocity, maxLinearVelocity, currVelocity.vel);
        limitVector(-maxAngularVelocity, maxAngularVelocity, currVelocity.rot);
    }
}

std::string Cartesian_HybCntrl::getSensorNameFromNode(const std::string& nodeName)
{
    for (std::map<std::string, std::string>::iterator itr = sensorNameMap.begin(); itr != sensorNameMap.end(); ++itr)
    {
        if (nodeName.find(itr->first) != std::string::npos)
        {
            return itr->second;
        }
    }

    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << "Could not find sensor for " << nodeName << "!!";
    return "";
}