jaco_types.cpp

Go to the documentation of this file.

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

namespace jaco
{

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


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 = (float) pose.position.x;
        Y = (float) pose.position.y;
        Z = (float) pose.position.z;

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

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

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

geometry_msgs::Pose JacoPose::Pose()
{
        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::Compare(const JacoPose &other, float tolerance) const
{
        bool status = true;

        status = status && CompareValues(X, other.X, tolerance);
        status = status && CompareValues(Y, other.Y, tolerance);
        status = status && CompareValues(Z, other.Z, tolerance);
        status = status && CompareValues(ThetaX, other.ThetaX, tolerance);
        status = status && CompareValues(ThetaY, other.ThetaY, tolerance);
        status = status && CompareValues(ThetaZ, other.ThetaZ, tolerance);

        return status;
}

float JacoPose::Normalize(float value)
{
        while (value > 2 * M_PI)
                value -= 2 * M_PI;
        while (value < 0)
                value += 2 * M_PI;

        return value;
}

JacoAngles::JacoAngles(const jaco_driver::JointAngles &angles)
{
        Actuator1 = Normalize(180.0 - (angles.Angle_J1 * (180.0 / M_PI)));
        Actuator2 = Normalize((angles.Angle_J2 * (180.0 / M_PI)) + 270.0);
        Actuator3 = Normalize(90.0 - (angles.Angle_J3 * (180.0 / M_PI)));
        Actuator4 = Normalize(180.0 - (angles.Angle_J4 * (180.0 / M_PI)));
        Actuator5 = Normalize(180.0 - (angles.Angle_J5 * (180.0 / M_PI)));
        Actuator6 = Normalize(260.0 - (angles.Angle_J6 * (180.0 / M_PI)));
}

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

jaco_driver::JointAngles JacoAngles::Angles()
{
        jaco_driver::JointAngles angles;
        angles.Angle_J1 = (180.0 - Actuator1) / (180.0 / M_PI);
        angles.Angle_J2 = (Actuator2 - 270.0) / (180.0 / M_PI);
        angles.Angle_J3 = (90.0 - Actuator3) / (180.0 / M_PI);
        angles.Angle_J4 = (180.0 - Actuator4) / (180.0 / M_PI);
        angles.Angle_J5 = (180.0 - Actuator5) / (180.0 / M_PI);
        angles.Angle_J6 = (260.0 - Actuator6) / (180.0 / M_PI);

        return angles;
}

bool JacoAngles::Compare(const JacoAngles &other, float tolerance) const 
{
        bool status = true;

        status = status && CompareValues(Actuator1, other.Actuator1, tolerance);
        status = status && CompareValues(Actuator2, other.Actuator2, tolerance);
        status = status && CompareValues(Actuator3, other.Actuator3, tolerance);
        status = status && CompareValues(Actuator4, other.Actuator4, tolerance);
        status = status && CompareValues(Actuator5, other.Actuator5, tolerance);
        status = status && CompareValues(Actuator6, other.Actuator6, tolerance);

        return status;
}

float JacoAngles::Normalize(float value)
{
        while (value > 360.0)
                value -= 360.0;
        while (value < 0.0)
                value += 360.0;

        return value;
}

FingerAngles::FingerAngles(const jaco_driver::FingerPosition &position)
{
        Finger1 = position.Finger_1;
        Finger2 = position.Finger_2;
        Finger3 = position.Finger_3;
}

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

jaco_driver::FingerPosition FingerAngles::Fingers()
{
        jaco_driver::FingerPosition angles;
        angles.Finger_1 = Finger1;
        angles.Finger_2 = Finger2;
        angles.Finger_3 = Finger3;

        return angles;
}

bool FingerAngles::Compare(const FingerAngles &other, float tolerance) const
{
        bool status = true;

        status = status && CompareValues(Finger1, other.Finger1, tolerance);
        status = status && CompareValues(Finger2, other.Finger2, tolerance);
        status = status && CompareValues(Finger3, other.Finger3, tolerance);

        return status;
}

}