dynamixel_gripper_driver.cpp

Go to the documentation of this file.

#include "dynamixel_gripper_driver.h"

DynamixelGripperDriver::DynamixelGripperDriver() :
    motor_(),
    baudrate_(1000000),
    bus_id_(0),
    servo_id_(255),
    max_angle_(170), // open angle (could be higher)
    min_angle_(115), // close angle
    effort_(10.0),
    closed_(false)
{ }

bool
DynamixelGripperDriver::openDriver()
{
    std::vector<bool> enable(1,true);
    std::string motor_name="gripper_motor";

    if (servo_id_ == 255) {
        ROS_WARN("No servo identifier has been set");
        return false;
    }

    try
    {
      motor_ = boost::shared_ptr<CDynamixelMotor>(new CDynamixelMotor(motor_name, bus_id_, baudrate_, servo_id_));
      // configure the motor
      motor_->enable(enable);
      motor_->set_torque(effort_);
    }
    catch(CException &e)
    {
        ROS_ERROR("%s",e.what().c_str());
        return false;
    }

    ROS_INFO("Gripper is ready");

    return true;
}

bool
DynamixelGripperDriver::closeDriver()
{
    motor_->close();
    return true;
}

bool
DynamixelGripperDriver::startDriver()
{
    return true;
}

bool
DynamixelGripperDriver::stopDriver()
{
    return true;
}

void
DynamixelGripperDriver::config_update(Config& new_cfg, uint32_t level)
{
    std::cout << "Config update" << std::endl;
    this->lock();

    switch(this->getState())
    {
        case DynamixelGripperDriver::CLOSED:
            servo_id_  = new_cfg.servo_id;
            bus_id_    = new_cfg.bus_id;
            baudrate_  = new_cfg.baudrate;
            max_angle_ = new_cfg.max_angle;
            min_angle_ = new_cfg.min_angle;
            effort_    = new_cfg.effort;
            break;

        case DynamixelGripperDriver::OPENED:
            break;

        case DynamixelGripperDriver::RUNNING:
            try
            {
                motor_->set_torque(new_cfg.effort);
                max_angle_ = new_cfg.max_angle;
                min_angle_ = new_cfg.min_angle;
                effort_    = new_cfg.effort;
            }
            catch(CException &e)
            {
              ROS_ERROR("%s",e.what().c_str());
              new_cfg.max_angle = max_angle_;
              new_cfg.min_angle = min_angle_;
              new_cfg.effort    = effort_;
            }
            break;
    }

    // save the current configuration
    this->config_=new_cfg;

    this->unlock();
}

bool
DynamixelGripperDriver::open_gripper()
{
    return move_gripper(max_angle_);
}

bool
DynamixelGripperDriver::close_gripper()
{
    return move_gripper(min_angle_);
}

void
DynamixelGripperDriver::stop_gripper()
{
    if (this->getState() != DynamixelGripperDriver::CLOSED)
        motor_->stop();
}

bool
DynamixelGripperDriver::is_moving()
{
    if (this->getState() == DynamixelGripperDriver::CLOSED)
        return false;

    bool response = true;
    // do not delete the pointer at the end of the function. It will crash.
    CEventServer * event_server = CEventServer::instance();

    if (event_server->event_is_set(motor_->get_position_feedback_event()))
        response = false;

    return response;
}

float
DynamixelGripperDriver::get_effort()
{
    if (this->getState() == DynamixelGripperDriver::CLOSED)
        return 0.0;

    return motor_->get_torque();
}

float
DynamixelGripperDriver::get_max_effort()
{
    return effort_;
}

float
DynamixelGripperDriver::get_angle()
{
    if (this->getState() == DynamixelGripperDriver::CLOSED)
        return 0.0;

    return motor_->get_position()[0];
}

bool
DynamixelGripperDriver::move_gripper(const double position_in_radians)
{
    std::vector<float> position(1),velocity(1),accel(1);
    std::vector<float> current_pos(1);

    if (this->getState() == DynamixelGripperDriver::CLOSED) {
        ROS_WARN("Open driver action was tried without having the driver CLOSED");
        return false;
    }

    std::cout << "Moving the gripper to: " << position_in_radians <<  std::endl;

    /* stop any previous action */
    motor_->stop();

    /* load the next motion */
    position[0] = position_in_radians;
    velocity[0] = 100.0;
    current_pos = motor_->get_position();

    // do not move if the motor is almost there
    if (fabs(current_pos[0] - position[0]) > 1.0) {
        accel[0] = velocity[0] * velocity[0]/(2*0.1*fabs(position[0]-current_pos[0]));
        std::cout << "Position: " << position[0] <<
                     "Vel: "      << velocity[0] <<
                     "Acc: "      << accel[0] <<  std::endl;
        motor_->load(position,velocity,accel);
        motor_->move();
    }

    return true;
}

DynamixelGripperDriver::~DynamixelGripperDriver()
{
    motor_->close();
}