jaco_comm.cpp

Go to the documentation of this file.

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


namespace jaco
{

JacoComm::JacoComm(const ros::NodeHandle& node_handle,
                   boost::recursive_mutex &api_mutex,
                   const bool is_movement_on_start)
    : is_software_stop_(false), api_mutex_(api_mutex)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    // Get the serial number parameter for the arm we wish to connec to
    std::string serial_number = "";
    node_handle.getParam("serial_number", serial_number);

    std::vector<int> api_version;
    int result = jaco_api_.getAPIVersion(api_version);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get the Kinova API version", result);
    }

    ROS_INFO_STREAM("Initializing Kinova API (header version: " << COMMAND_LAYER_VERSION << ", library version: "
                    << api_version[0] << "." << api_version[1] << "." << api_version[2] << ")");

    result = jaco_api_.initAPI();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not initialize Kinova API", result);
    }

    std::vector<KinovaDevice> devices_list;
    result = NO_ERROR_KINOVA;
    jaco_api_.getDevices(devices_list, result);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get devices list", result);
    }

    bool found_arm = false;
    for (int device_i = 0; device_i < devices_list.size(); device_i++)
    {
        // If no device is specified, just use the first available device
        if ((serial_number == "")
            || (std::strcmp(serial_number.c_str(), devices_list[device_i].SerialNumber) == 0))
        {
            result = jaco_api_.setActiveDevice(devices_list[device_i]);
            if (result != NO_ERROR_KINOVA)
            {
                throw JacoCommException("Could not set the active device", result);
            }

            GeneralInformations general_info;
            result = jaco_api_.getGeneralInformations(general_info);
            if (result != NO_ERROR_KINOVA)
            {
                throw JacoCommException("Could not get general information about the device", result);
            }

            ClientConfigurations configuration;
            getConfig(configuration);

            QuickStatus quick_status;
            getQuickStatus(quick_status);

            if ((quick_status.RobotType != 0) && (quick_status.RobotType != 1))
            {
                ROS_ERROR("Could not get the type of the arm from the quick status, expected "
                          "either type 0 (JACO), or type 1 (MICO), got %d", quick_status.RobotType);
                throw JacoCommException("Could not get the type of the arm", quick_status.RobotType);
            }

            num_fingers_ = quick_status.RobotType == 0 ? 3 : 2;

            ROS_INFO_STREAM("Found " << devices_list.size() << " device(s), using device at index " << device_i
                            << " (model: " << configuration.Model
                            << ", serial number: " << devices_list[device_i].SerialNumber
                            << ", code version: " << general_info.CodeVersion
                            << ", code revision: " << general_info.CodeRevision << ")");

            found_arm = true;
            break;
        }
    }

    if (!found_arm)
    {
        ROS_ERROR("Could not find the specified arm (serial: %s) among the %d attached devices",
                  serial_number.c_str(), static_cast<int>(devices_list.size()));
        throw JacoCommException("Could not find the specified arm", 0);
    }

    // On a cold boot the arm may not respond to commands from the API right away.
    // This kick-starts the Control API so that it's ready to go.
    startAPI();
    stopAPI();
    startAPI();

    // Set the angular velocity of each of the joints to zero
    TrajectoryPoint jaco_velocity;
    memset(&jaco_velocity, 0, sizeof(jaco_velocity));
    setCartesianVelocities(jaco_velocity.Position.CartesianPosition);

    if (is_movement_on_start)
    {
        initFingers();
    }
    else
    {
        ROS_WARN("Movement on connection to the arm has been suppressed on initialization. You may "
                 "have to home the arm (through the home service) before movement is possible");
    }
}


JacoComm::~JacoComm()
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    jaco_api_.closeAPI();
}


bool JacoComm::isHomed(void)
{
    QuickStatus quick_status;
    getQuickStatus(quick_status);

    if (quick_status.RetractType == 1)
    {
        return true;
    }
    else
    {
        return false;
    }
}


void JacoComm::homeArm(void)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    if (isStopped())
    {
        ROS_INFO("Arm is stopped, cannot home");
        return;
    }
    else if (isHomed())
    {
        ROS_INFO("Arm is already in \"home\" position");
        return;
    }

    stopAPI();
    ros::Duration(1.0).sleep();
    startAPI();

    ROS_INFO("Homing the arm");
    int result = jaco_api_.moveHome();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Move home failed", result);
    }
}


void JacoComm::initFingers(void)
{
    ROS_INFO("Initializing fingers...this will take a few seconds and the fingers should open completely");
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    int result = jaco_api_.initFingers();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not init fingers", result);
    }
    return;
}


void JacoComm::setJointAngles(const JacoAngles &angles, int timeout, bool push)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    if (isStopped())
    {
        ROS_INFO("The angles could not be set because the arm is stopped");
        return;
    }

    int result = NO_ERROR_KINOVA;
    TrajectoryPoint jaco_position;
    jaco_position.InitStruct();
    memset(&jaco_position, 0, sizeof(jaco_position));  // zero structure

    if (push)
    {
        result = jaco_api_.eraseAllTrajectories();
        if (result != NO_ERROR_KINOVA)
        {
            throw JacoCommException("Could not erase trajectories", result);
        }
    }

    startAPI();

    result = jaco_api_.setAngularControl();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not set angular control", result);
    }

    jaco_position.Position.Delay = 0.0;
    jaco_position.Position.Type = ANGULAR_POSITION;
    jaco_position.Position.Actuators = angles;

    result = jaco_api_.sendAdvanceTrajectory(jaco_position);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not send advanced joint angle trajectory", result);
    }
}


void JacoComm::setCartesianPosition(const JacoPose &position, int timeout, bool push)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    if (isStopped())
    {
        ROS_INFO("The position could not be set because the arm is stopped");
        return;
    }

    int result = NO_ERROR_KINOVA;
    TrajectoryPoint jaco_position;
    jaco_position.InitStruct();
    memset(&jaco_position, 0, sizeof(jaco_position));  // zero structure

    if (push)
    {
        result = jaco_api_.eraseAllTrajectories();
        if (result != NO_ERROR_KINOVA)
        {
            throw JacoCommException("Could not erase trajectories", result);
        }
    }

    startAPI();

    result = jaco_api_.setCartesianControl();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not set Cartesian control", result);
    }

    jaco_position.Position.Delay = 0.0;
    jaco_position.Position.Type = CARTESIAN_POSITION;
    jaco_position.Position.HandMode = HAND_NOMOVEMENT;

    // These values will not be used but are initialized anyway.
    jaco_position.Position.Actuators.Actuator1 = 0.0f;
    jaco_position.Position.Actuators.Actuator2 = 0.0f;
    jaco_position.Position.Actuators.Actuator3 = 0.0f;
    jaco_position.Position.Actuators.Actuator4 = 0.0f;
    jaco_position.Position.Actuators.Actuator5 = 0.0f;
    jaco_position.Position.Actuators.Actuator6 = 0.0f;

    jaco_position.Position.CartesianPosition = position;

    result = jaco_api_.sendBasicTrajectory(jaco_position);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not send basic trajectory", result);
    }
}


void JacoComm::setFingerPositions(const FingerAngles &fingers, int timeout, bool push)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    if (isStopped())
    {
        ROS_INFO("The fingers could not be set because the arm is stopped");
        return;
    }

    int result = NO_ERROR_KINOVA;
    TrajectoryPoint jaco_position;
    jaco_position.InitStruct();
    memset(&jaco_position, 0, sizeof(jaco_position));  // zero structure

    if (push)
    {
        result = jaco_api_.eraseAllTrajectories();
        if (result != NO_ERROR_KINOVA)
        {
            throw JacoCommException("Could not erase trajectories", result);
        }
    }

    startAPI();

    result = jaco_api_.setCartesianControl();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not set Cartesian control", result);
    }

    // Initialize Cartesian control of the fingers
    jaco_position.Position.HandMode = POSITION_MODE;
    jaco_position.Position.Type = CARTESIAN_POSITION;
    jaco_position.Position.Fingers = fingers;
    jaco_position.Position.Delay = 0.0;
    jaco_position.LimitationsActive = 0;

    // These values will not be used but are initialized anyway.
    JacoAngles angles;
    getJointAngles(angles);
    jaco_position.Position.Actuators = angles;

    // When loading a cartesian position for the fingers, values are required for the arm joints
    // as well or the arm goes nuts.  Grab the current position and feed it back to the arm.
    JacoPose pose;
    getCartesianPosition(pose);
    jaco_position.Position.CartesianPosition = pose;

    result = jaco_api_.sendAdvanceTrajectory(jaco_position);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not send advanced finger trajectory", result);
    }
}


void JacoComm::setJointVelocities(const AngularInfo &joint_vel)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    if (isStopped())
    {
        ROS_INFO("The velocities could not be set because the arm is stopped");
        return;
    }

    TrajectoryPoint jaco_velocity;
    jaco_velocity.InitStruct();

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

    startAPI();
    jaco_velocity.Position.Type = ANGULAR_VELOCITY;

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

    int result = jaco_api_.sendAdvanceTrajectory(jaco_velocity);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not send advanced joint velocity trajectory", result);
    }
}


void JacoComm::setCartesianVelocities(const CartesianInfo &velocities)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);

    if (isStopped())
    {
        ROS_INFO("The cartesian velocities could not be set because the arm is stopped");
        jaco_api_.eraseAllTrajectories();
        return;
    }

    TrajectoryPoint jaco_velocity;
    jaco_velocity.InitStruct();

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

    startAPI();
    jaco_velocity.Position.Type = CARTESIAN_VELOCITY;

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

    int result = jaco_api_.sendAdvanceTrajectory(jaco_velocity);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not send advanced Cartesian velocity trajectory", result);
    }
}


void JacoComm::setConfig(const ClientConfigurations &config)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    int result = jaco_api_.setClientConfigurations(config);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not set the client configuration", result);
    }
}


void JacoComm::getJointAngles(JacoAngles &angles)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    AngularPosition jaco_angles;
    memset(&jaco_angles, 0, sizeof(jaco_angles));  // zero structure

    int result = jaco_api_.getAngularPosition(jaco_angles);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get the angular position", result);
    }

    angles = JacoAngles(jaco_angles.Actuators);
}


void JacoComm::getCartesianPosition(JacoPose &position)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    CartesianPosition jaco_cartesian_position;
    memset(&jaco_cartesian_position, 0, sizeof(jaco_cartesian_position));  // zero structure

    int result = jaco_api_.getCartesianPosition(jaco_cartesian_position);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get the Cartesian position", result);
    }

    position = JacoPose(jaco_cartesian_position.Coordinates);
}


void JacoComm::getFingerPositions(FingerAngles &fingers)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    CartesianPosition jaco_cartesian_position;
    memset(&jaco_cartesian_position, 0, sizeof(jaco_cartesian_position));  // zero structure

    int result = jaco_api_.getCartesianPosition(jaco_cartesian_position);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get Cartesian finger position", result);
    }

    if (num_fingers_ == 2)
    {
        jaco_cartesian_position.Fingers.Finger3 = 0.0;
    }

    fingers = FingerAngles(jaco_cartesian_position.Fingers);
}


void JacoComm::getConfig(ClientConfigurations &config)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    memset(&config, 0, sizeof(config));  // zero structure

    int result = jaco_api_.getClientConfigurations(config);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get client configuration", result);
    }
}


void JacoComm::getQuickStatus(QuickStatus &quick_status)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    memset(&quick_status, 0, sizeof(quick_status));  // zero structure
    int result = jaco_api_.getQuickStatus(quick_status);
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not get quick status", result);
    }
}


void JacoComm::stopAPI()
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    is_software_stop_ = true;

    int result = jaco_api_.stopControlAPI();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not stop the control API", result);
    }

    result = jaco_api_.eraseAllTrajectories();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not erase all trajectories", result);
    }
}


void JacoComm::startAPI()
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    if (is_software_stop_)
    {
        is_software_stop_ = false;
        jaco_api_.stopControlAPI();
        ros::Duration(0.05).sleep();
    }

    int result = jaco_api_.startControlAPI();
    if (result != NO_ERROR_KINOVA)
    {
        throw JacoCommException("Could not start the control API", result);
    }
}


int JacoComm::numFingers()
{
    return num_fingers_;
}


void JacoComm::printAngles(const JacoAngles &angles)
{
    ROS_INFO("Joint angles (deg) -- J1: %f, J2: %f J3: %f, J4: %f, J5: %f, J6: %f",
             angles.Actuator1, angles.Actuator2, angles.Actuator3,
             angles.Actuator4, angles.Actuator5, angles.Actuator6);
}


void JacoComm::printPosition(const JacoPose &position)
{
    ROS_INFO("Arm position\n"
             "\tposition (m) -- x: %f, y: %f z: %f\n"
             "\trotation (rad) -- theta_x: %f, theta_y: %f, theta_z: %f",
             position.X, position.Y, position.Z,
             position.ThetaX, position.ThetaY, position.ThetaZ);
}


void JacoComm::printFingers(const FingersPosition &fingers)
{
    ROS_INFO("Finger positions -- F1: %f, F2: %f, F3: %f",
             fingers.Finger1, fingers.Finger2, fingers.Finger3);
}


void JacoComm::printConfig(const ClientConfigurations &config)
{
    ROS_INFO_STREAM("Arm configuration:\n"
                    "\tClientID: " << config.ClientID <<
                    "\n\tClientName: " << config.ClientName <<
                    "\n\tOrganization: " << config.Organization <<
                    "\n\tSerial:" << config.Serial <<
                    "\n\tModel: " << config.Model <<
                    "\n\tMaxForce: " << config.MaxForce <<
                    "\n\tSensibility: " <<  config.Sensibility <<
                    "\n\tDrinkingHeight: " << config.DrinkingHeight <<
                    "\n\tComplexRetractActive: " << config.ComplexRetractActive <<
                    "\n\tRetractedPositionAngle: " << config.RetractedPositionAngle <<
                    "\n\tRetractedPositionCount: " << config.RetractedPositionCount <<
                    "\n\tDrinkingDistance: " << config.DrinkingDistance <<
                    "\n\tFingers2and3Inverted: " << config.Fingers2and3Inverted <<
                    "\n\tDrinkingLength: " << config.DrinkingLenght <<
                    "\n\tDeletePreProgrammedPositionsAtRetract: " <<
                    config.DeletePreProgrammedPositionsAtRetract <<
                    "\n\tEnableFlashErrorLog: " << config.EnableFlashErrorLog <<
                    "\n\tEnableFlashPositionLog: " << config.EnableFlashPositionLog);
}


bool JacoComm::isStopped()
{
    return is_software_stop_;
}


}  // namespace jaco