jaco_types.cpp

Go to the documentation of this file.

#include <math.h>
#include <angles/angles.h>
#include <tf/tf.h>
#include <jaco_driver/jaco_types.h>
#include <string>


namespace jaco
{

// A few helper functions
// ----------------------

float normalizeInRads(float rads)
{
    return static_cast<float>(angles::normalize_angle_positive(rads));
}


float normalizePositiveInDegrees(float degrees)
{
    return angles::to_degrees(angles::normalize_angle_positive(angles::from_degrees(degrees)));
}


float normalizeInDegrees(float degrees)
{
    return angles::to_degrees(angles::normalize_angle(angles::from_degrees(degrees)));
}


bool areValuesClose(float first, float second, float tolerance)
{
    return ((first <= second + tolerance) && (first >= second - tolerance));
}


// Exceptions
// ----------
JacoCommException::JacoCommException(const std::string& message, const int error_code)
{
    std::stringstream ss;
        ss << "JacoCommException: " << message << " (return code: " << error_code << ")" << std::endl;
    desc_ = ss.str();
}


const char* JacoCommException::what() const throw()
{
    return desc_.c_str();
}


// Class definitions
// -----------------

JacoPose::JacoPose(const geometry_msgs::Pose &pose)
{
    double tx, ty, tz;
    tf::Quaternion q;
    tf::quaternionMsgToTF(pose.orientation, q);

    tf::Matrix3x3 bt_q(q);

    bt_q.getEulerYPR(tz, ty, tx);

    X = static_cast<float>(pose.position.x);
    Y = static_cast<float>(pose.position.y);
    Z = static_cast<float>(pose.position.z);

    ThetaX = normalizeInRads(tx);
    ThetaY = normalizeInRads(ty);
    ThetaZ = normalizeInRads(tz);
}


JacoPose::JacoPose(const CartesianInfo &pose)
{
    X = pose.X;
    Y = pose.Y;
    Z = pose.Z;

    ThetaX = normalizeInRads(pose.ThetaX);
    ThetaY = normalizeInRads(pose.ThetaY);
    ThetaZ = normalizeInRads(pose.ThetaZ);
}


geometry_msgs::Pose JacoPose::constructPoseMsg()
{
    geometry_msgs::Pose pose;
    tf::Quaternion position_quaternion;

    position_quaternion.setRPY(ThetaX, ThetaY, ThetaZ);
    tf::quaternionTFToMsg(position_quaternion, pose.orientation);

    pose.position.x = X;
    pose.position.y = Y;
    pose.position.z = Z;

    return pose;
}


bool JacoPose::isCloseToOther(const JacoPose &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(X, other.X, tolerance);
    status = status && areValuesClose(Y, other.Y, tolerance);
    status = status && areValuesClose(Z, other.Z, tolerance);
    status = status && areValuesClose(ThetaX, other.ThetaX, tolerance);
    status = status && areValuesClose(ThetaY, other.ThetaY, tolerance);
    status = status && areValuesClose(ThetaZ, other.ThetaZ, tolerance);
    return status;
}


JacoAngles::JacoAngles(const jaco_msgs::JointAngles &angles)
{
    Actuator1 = 180.0 - (angles.joint1 * (180.0 / M_PI));
    Actuator2 = normalizePositiveInDegrees((angles.joint2 * (180.0 / M_PI)) + 270.0);
    Actuator3 = normalizePositiveInDegrees(90.0 - (angles.joint3 * (180.0 / M_PI)));
    Actuator4 = 180.0 - (angles.joint4 * (180.0 / M_PI));
    Actuator5 = 180.0 - (angles.joint5 * (180.0 / M_PI));
    Actuator6 = 260.0 - (angles.joint6 * (180.0 / M_PI));
}


JacoAngles::JacoAngles(const AngularInfo &angles)
{
    Actuator1 = angles.Actuator1;
    Actuator2 = normalizePositiveInDegrees(angles.Actuator2);
    Actuator3 = normalizePositiveInDegrees(angles.Actuator3);
    Actuator4 = angles.Actuator4;
    Actuator5 = angles.Actuator5;
    Actuator6 = angles.Actuator6;
}


jaco_msgs::JointAngles JacoAngles::constructAnglesMsg()
{
    jaco_msgs::JointAngles angles;
    angles.joint1 = (180.0 - Actuator1) / (180.0 / M_PI);
    angles.joint2 = (Actuator2 - 270.0) / (180.0 / M_PI);
    angles.joint3 = (90.0 - Actuator3) / (180.0 / M_PI);
    angles.joint4 = (180.0 - Actuator4) / (180.0 / M_PI);
    angles.joint5 = (180.0 - Actuator5) / (180.0 / M_PI);
    angles.joint6 = (260.0 - Actuator6) / (180.0 / M_PI);
    return angles;
}


bool JacoAngles::isCloseToOther(const JacoAngles &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(Actuator1, other.Actuator1, tolerance);
    status = status && areValuesClose(Actuator2, other.Actuator2, tolerance);
    status = status && areValuesClose(Actuator3, other.Actuator3, tolerance);
    status = status && areValuesClose(Actuator4, other.Actuator4, tolerance);
    status = status && areValuesClose(Actuator5, other.Actuator5, tolerance);
    status = status && areValuesClose(Actuator6, other.Actuator6, tolerance);
    return status;
}


FingerAngles::FingerAngles(const jaco_msgs::FingerPosition &position)
{
    Finger1 = position.finger1;
    Finger2 = position.finger2;
    Finger3 = position.finger3;
}


FingerAngles::FingerAngles(const FingersPosition &angle)
{
    Finger1 = angle.Finger1;
    Finger2 = angle.Finger2;
    Finger3 = angle.Finger3;
}


jaco_msgs::FingerPosition FingerAngles::constructFingersMsg()
{
    jaco_msgs::FingerPosition angles;
    angles.finger1 = Finger1;
    angles.finger2 = Finger2;
    angles.finger3 = Finger3;
    return angles;
}


bool FingerAngles::isCloseToOther(const FingerAngles &other, float tolerance) const
{
    bool status = true;
    status = status && areValuesClose(Finger1, other.Finger1, tolerance);
    status = status && areValuesClose(Finger2, other.Finger2, tolerance);
    status = status && areValuesClose(Finger3, other.Finger3, tolerance);
    return status;
}

}  // namespace jaco