jaco_comm.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include "jaco_driver/jaco_comm.h"

#define PI 3.14159265358

namespace jaco
{

JacoComm::JacoComm(JacoAngles home) : software_stop(false), home_position(home)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        /* Connecting to Jaco Arm */
        ROS_INFO("Initiating Library");
        API = new JacoAPI();
        ROS_INFO("Initiating API");

        int api_result = 0; //stores result from the API
        ros::Duration(1.0).sleep();

        api_result = (API->InitAPI());

        /* 
        A common result that may be returned is "1014", which means communications
        could not be established with the arm.  This often means the arm is not turned on, 
        or the InitAPI command was initiated before the arm had fully booted up.
        */

        if (api_result != 1)
        {
                /* Failed to contact arm */
                ROS_FATAL("Could not initialize arm");
                ROS_FATAL("Jaco_InitAPI returned: %d", api_result);
                #ifndef DEBUG_WITHOUT_ARM
                ros::shutdown();
                #endif
        } 
        else
        {
                ROS_INFO("API Initialized Successfully!");
        }
}

JacoComm::~JacoComm()
{
        API->CloseAPI();
}

bool JacoComm::HomeState(void)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        const float tolerance = 2.0; //dead zone for angles (degrees)

        AngularPosition cur_angles; //holds the current angles of the arm
        API->GetAngularPosition(cur_angles); //update current arm angles

        return home_position.Compare(JacoAngles(cur_angles.Actuators), tolerance);
}

void JacoComm::HomeArm(void)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        if (Stopped())
        {
                return;
        }

        if (HomeState())
        {
                ROS_INFO("Arm is already in \"home\" position");
                return;
        }

        API->StartControlAPI();

        JoystickCommand home_command;
        memset(&home_command, 0, sizeof(home_command)); //zero structure

        home_command.ButtonValue[2] = 1;
        API->SendJoystickCommand(home_command);

        WaitForHome(25);

        home_command.ButtonValue[2] = 0;
        API->SendJoystickCommand(home_command);
}

void JacoComm::InitializeFingers(void)
{
        FingerAngles fingers_home;

        // Set the fingers fully "open." This is required to initialize the fingers.
        SetFingers(fingers_home, 5);
        ros::Duration(3.0).sleep();

        // Set the fingers to "half-open"
        fingers_home.Finger1 = 40;
        fingers_home.Finger2 = 40;
        fingers_home.Finger3 = 40;
        SetFingers(fingers_home, 5);
}

void JacoComm::SetAngles(JacoAngles &angles, int timeout, bool push)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        if (Stopped())
                return;

        TrajectoryPoint Jaco_Position;

        memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure

        if (push == true)
        {
                API->EraseAllTrajectories();
                API->StopControlAPI();
        }
        
        API->StartControlAPI();
        API->SetAngularControl();
        
        //Jaco_Position.LimitationsActive = false;
        Jaco_Position.Position.Delay = 0.0;
        Jaco_Position.Position.Type = ANGULAR_POSITION;

        Jaco_Position.Position.Actuators = angles; 

        API->SendAdvanceTrajectory(Jaco_Position);
}

void JacoComm::SetPosition(JacoPose &position, int timeout, bool push)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        if (Stopped())
                return;

        TrajectoryPoint Jaco_Position;

        memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure

        if (push == true)
        {
                API->EraseAllTrajectories();
                API->StopControlAPI();
        }

        API->StartControlAPI();
        API->SetCartesianControl();

        //Jaco_Position.LimitationsActive = false;
        Jaco_Position.Position.Delay = 0.0;
        Jaco_Position.Position.Type = CARTESIAN_POSITION;

        Jaco_Position.Position.CartesianPosition = position;
        //Jaco_Position.Position.CartesianPosition.ThetaZ += 0.0001; // A workaround for a bug in the Kinova API

        API->SendBasicTrajectory(Jaco_Position);
}

void JacoComm::SetFingers(FingerAngles &fingers, int timeout, bool push)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        if (Stopped())
                return;

        TrajectoryPoint Jaco_Position;
        memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure

        if (push == true)
        {
                API->EraseAllTrajectories();
                API->StopControlAPI();
        }

        API->StartControlAPI();

        Jaco_Position.Position.HandMode = POSITION_MODE;
        Jaco_Position.Position.Fingers = fingers;

        API->SendAdvanceTrajectory(Jaco_Position);
        ROS_DEBUG("Sending Fingers");
}

void JacoComm::SetVelocities(AngularInfo joint_vel)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        if (Stopped())
                return;

        TrajectoryPoint Jaco_Velocity;

        memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure

        API->StartControlAPI();
        Jaco_Velocity.Position.Type = ANGULAR_VELOCITY;

        // confusingly, velocity is passed in the position struct
        Jaco_Velocity.Position.Actuators = joint_vel;

        API->SendAdvanceTrajectory(Jaco_Velocity);
}

void JacoComm::SetCartesianVelocities(CartesianInfo velocities)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        if (Stopped())
        {
                API->EraseAllTrajectories();
                return;
        }

        TrajectoryPoint Jaco_Velocity;

        memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure

        API->StartControlAPI();
        Jaco_Velocity.Position.Type = CARTESIAN_VELOCITY;

        // confusingly, velocity is passed in the position struct
        Jaco_Velocity.Position.CartesianPosition = velocities;

        API->SendAdvanceTrajectory(Jaco_Velocity);
}

void JacoComm::SetConfig(ClientConfigurations config)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        API->SetClientConfigurations(config);
}

void JacoComm::GetAngles(JacoAngles &angles)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);

        AngularPosition Jaco_Position;
        API->GetAngularPosition(Jaco_Position);

        angles = Jaco_Position.Actuators;
}

void JacoComm::GetPosition(JacoPose &position)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        CartesianPosition Jaco_Position;

        memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure

        API->GetCartesianPosition(Jaco_Position);

        position = JacoPose(Jaco_Position.Coordinates);
}

void JacoComm::GetFingers(FingerAngles &fingers)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        CartesianPosition Jaco_Position;

        API->GetCartesianPosition(Jaco_Position);

        fingers = Jaco_Position.Fingers;
}

void JacoComm::GetConfig(ClientConfigurations &config)
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        memset(&config, 0, sizeof(config)); //zero structure
        API->GetClientConfigurations(config);
}

void JacoComm::PrintAngles(JacoAngles &angles)
{
        ROS_INFO("Jaco Arm Angles (Degrees)");
        ROS_INFO("Joint 1 = %f", angles.Actuator1);
        ROS_INFO("Joint 2 = %f", angles.Actuator2);
        ROS_INFO("Joint 3 = %f", angles.Actuator3);

        ROS_INFO("Joint 4 = %f", angles.Actuator4);
        ROS_INFO("Joint 5 = %f", angles.Actuator5);
        ROS_INFO("Joint 6 = %f", angles.Actuator6);
}

void JacoComm::PrintPosition(JacoPose &position)
{
        ROS_INFO("Jaco Arm Position (Meters)");
        ROS_INFO("X = %f", position.X);
        ROS_INFO("Y = %f", position.Y);
        ROS_INFO("Z = %f", position.Z);

        ROS_INFO("Jaco Arm Rotations (Radians)");
        ROS_INFO("Theta X = %f", position.ThetaX);
        ROS_INFO("Theta Y = %f", position.ThetaY);
        ROS_INFO("Theta Z = %f", position.ThetaZ);
}

void JacoComm::PrintFingers(FingersPosition fingers)
{
        ROS_INFO("Jaco Arm Finger Positions");
        ROS_INFO("Finger 1 = %f", fingers.Finger1);
        ROS_INFO("Finger 2 = %f", fingers.Finger2);
        ROS_INFO("Finger 3 = %f", fingers.Finger3);
}

void JacoComm::PrintConfig(ClientConfigurations config)
{
        ROS_INFO("Jaco Config");
        ROS_INFO("ClientID = %s", config.ClientID);
        ROS_INFO("ClientName = %s", config.ClientName);
        ROS_INFO("Organization = %s", config.Organization);
        ROS_INFO("Serial = %s", config.Serial);
        ROS_INFO("Model = %s", config.Model);
        ROS_INFO("MaxLinearSpeed = %f", config.MaxLinearSpeed);
        ROS_INFO("MaxAngularSpeed = %f", config.MaxAngularSpeed);
        ROS_INFO("MaxLinearAcceleration = %f", config.MaxLinearAcceleration);
        ROS_INFO("MaxForce = %f", config.MaxForce);
        ROS_INFO("Sensibility = %f", config.Sensibility);
        ROS_INFO("DrinkingHeight = %f", config.DrinkingHeight);
        ROS_INFO("ComplexRetractActive = %d", config.ComplexRetractActive);
        ROS_INFO("RetractedPositionAngle = %f", config.RetractedPositionAngle);
        ROS_INFO("RetractedPositionCount = %d", config.RetractedPositionCount);
        ROS_INFO("DrinkingDistance = %f", config.DrinkingDistance);
        ROS_INFO("Fingers2and3Inverted = %d", config.Fingers2and3Inverted);
        ROS_INFO("DrinkingLength = %f", config.DrinkingLenght);
        ROS_INFO("DeletePreProgrammedPositionsAtRetract = %d", config.DeletePreProgrammedPositionsAtRetract);
        ROS_INFO("EnableFlashErrorLog = %d", config.EnableFlashErrorLog);
        ROS_INFO("EnableFlashPositionLog = %d", config.EnableFlashPositionLog);
}

void JacoComm::Stop()
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        software_stop = true;

        API->StartControlAPI();

        JoystickCommand home_command;
        memset(&home_command, 0, sizeof(home_command)); //zero structure

        home_command.ButtonValue[2] = 1;
        API->SendJoystickCommand(home_command);
        API->EraseAllTrajectories();
        ros::Duration(0.05).sleep();

        home_command.ButtonValue[2] = 0;
        API->SendJoystickCommand(home_command);
}

void JacoComm::Start()
{
        boost::recursive_mutex::scoped_lock lock(api_mutex);
        software_stop = false;

        API->StartControlAPI();

}

bool JacoComm::Stopped()
{
        return software_stop;
}

void JacoComm::WaitForHome(int timeout)
{
        double start_secs;
        double current_sec;

        //If ros is still running use rostime, else use system time
        if (ros::ok())
        {
                start_secs = ros::Time::now().toSec();
                current_sec = ros::Time::now().toSec();
        } 
        else
        {
                start_secs = (double) time(NULL);
                current_sec = (double) time(NULL);
        }

        //while we have not timed out
        while ((current_sec - start_secs) < timeout)
        {
                ros::Duration(0.5).sleep();
                
                //If ros is still running use rostime, else use system time
                if (ros::ok())
                {
                        current_sec = ros::Time::now().toSec();
            ros::spinOnce();
                } 
                else
                {
                        current_sec = (double) time(NULL);
                }

                if (HomeState())
                {
                        ros::Duration(1.0).sleep();  // Grants a bit more time for the arm to "settle"
                        return;
                }
        }

        ROS_WARN("Timed out waiting for arm to return \"home\"");
}

} // namespace jaco