base_controller.cpp

Go to the documentation of this file.

/************************************************************************
 *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
 *      All rights reserved.                                                                                            *
 ************************************************************************
 *      Redistribution and use in source and binary forms, with or without      *
 *      modification, are permitted provided that the following conditions      *
 *      are met:                                                                                                                        *
 *                                                                                                                                              *
 *              1.      Redistributions of source code must retain the above            *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer.                                                                                                     *
 *                                                                                                                                              *
 *              2.      Redistributions in binary form must reproduce the above         *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer in the documentation and/or other materials          *
 *                      provided with the distribution.                                                         *
 *                                                                                                                                              *
 *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
 *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
 *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
 *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
 *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
 *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
 *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
 *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
 *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
 *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
 *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
 *      DAMAGE.                                                                                                                         *
 *                                                                                                                                              *
 *      The views and conclusions contained in the software and                         *
 *      documentation are those of the authors and should not be                        *
 *      interpreted as representing official policies, either expressed or      *
 *      implied, of TU/e.                                                                                                       *
 ************************************************************************/

#include "amigo_gazebo/base_controller.h"
#include <pluginlib/class_list_macros.h>

#define WHEELRAD 0.06
#define DIS2CENT 0.28991378 // distance of wheel to center
#define HALFSQRT2 0.7071

PLUGINLIB_REGISTER_CLASS(BaseControllerPlugin,
                         controller::BaseController,
                         pr2_controller_interface::Controller)

using namespace controller;

const static double EPS = 1e-8;

BaseController::BaseController() {

    //set all variables to zero

    current_pos_x=0;
    current_pos_y=0;
    current_pos_phi=0;

    desired_pos_x=0;
    desired_pos_y=0;
    desired_pos_phi=0;

    ref_vel_last_.linear.x=0.0;
    ref_vel_last_.linear.y=0.0;
    ref_vel_last_.angular.z=0.0;
}

BaseController::~BaseController() {
}

bool BaseController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) {

    std::string wheel_inner_left_front, wheel_inner_right_front, wheel_inner_left_rear, wheel_inner_right_rear;

    //Get params from parameter server or set defaults
    n.param<double> ("max_translational_velocity", max_translational_velocity_,0.3);
    n.param<double> ("min_translational_velocity", trans_vel_min,0.0);
    n.param<double> ("max_rotational_velocity", max_rotational_velocity_, 0.5);
    n.param<double> ("min_rotational_velocity", rot_vel_min,0.0);
    n.param<double> ("max_translational_acceleration/x", max_accel_.linear.x, .25);
    n.param<double> ("max_translational_acceleration/y", max_accel_.linear.y, .25);
    n.param<double> ("max_rotational_acceleration", max_accel_.angular.z, .2);
    n.param<double> ("max_control_effort_x", u_x_max,1000.0);
    n.param<double> ("max_control_effort_y", u_y_max, 1000.0);
    n.param<double> ("max_control_effort_phi", u_phi_max,1000.0);
    n.param<double> ("timeout", timeout_, 1.0);

    // feedback
    n.param<double> ("Kp_x", Kp_x, 0.0);
    n.param<double> ("Kd_x", Kd_x, 0.0);

    n.param<double> ("Kp_y", Kp_y, 0.0);
    n.param<double> ("Kd_y", Kd_y, 0.0);

    n.param<double> ("Kp_phi", Kp_phi, 0.0);
    n.param<double> ("Kd_phi", Kd_phi, 0.0);

    // feed forward
    n.param<double> ("mass_x", mass_x, 0.0);
    n.param<double> ("mass_y", mass_y, 0.0);
    n.param<double> ("inertia_phi", inertia_phi, 0.0);

    n.param<double> ("damping_x", damping_x, 0.0);
    n.param<double> ("damping_y", damping_y, 0.0);
    n.param<double> ("damping_phi", damping_phi, 0.0);

    //wheel_inner_left_front
    if (!n.getParam("wheel_inner_left_front", wheel_inner_left_front))
    {
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_left_front_state = robot->getJointState(wheel_inner_left_front);
    if (!wheel_inner_left_front_state )
    {
        ROS_ERROR("BaseController could not find joint named '%s'",
                  wheel_inner_left_front.c_str());
        return false;
    }

    //wheel_inner_right_front
    if (!n.getParam("wheel_inner_right_front", wheel_inner_right_front))
    {
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_right_front_state = robot->getJointState(wheel_inner_right_front);
    if (!wheel_inner_right_front_state )
    {
        ROS_ERROR("BaseController could not find joint named '%s'",
                  wheel_inner_right_front.c_str());
        return false;
    }

    //wheel_inner_left_rear
    if (!n.getParam("wheel_inner_left_rear", wheel_inner_left_rear))
    {
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_left_rear_state = robot->getJointState(wheel_inner_left_rear);
    if (!wheel_inner_left_rear_state )
    {
        ROS_ERROR("BaseController could not find joint named '%s'", wheel_inner_left_rear.c_str());
        return false;
    }

    //wheel_inner_right_rear
    if (!n.getParam("wheel_inner_right_rear", wheel_inner_right_rear))
    {
        ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str());
        return false;
    }

    wheel_inner_right_rear_state = robot->getJointState(wheel_inner_right_rear);
    if (!wheel_inner_right_rear_state )
    {
        ROS_ERROR("BaseController could not find joint named '%s'", wheel_inner_right_rear.c_str());
        return false;
    }

    //set topic
    vel_sub = n.subscribe("/cmd_vel", 1000, &BaseController::VelCallback,this);

    // copy robot pointer so we can access time
    robot_ = robot;

    return true;
}

void BaseController::starting() {
    time_of_last_cycle_ = robot_->getTime();
}

void BaseController::update() {
    current_time = robot_->getTime();
    double dT = std::min<double>((current_time - time_of_last_cycle_).toSec(), 0.02);

    geometry_msgs::Twist ref_vel;

    //if no command received in timeout period
    if((current_time - cmd_received_timestamp_).toSec() > timeout_)
    {
        ref_vel.linear.x = 0;
        ref_vel.linear.y = 0;
        ref_vel.angular.z = 0;
    }
    else
    {
        //interpolate and limit command velocities
        ref_vel = interpolateCommand(ref_vel_last_, cmd_vel_, max_accel_, dT);
    }


    //wheel velocity conversion to current x, y and phi velocities
    geometry_msgs::Twist current_vel;
    current_vel.linear.x = 0.25*WHEELRAD*(wheel_inner_right_front_state->velocity_+wheel_inner_left_front_state->velocity_-wheel_inner_left_rear_state->velocity_-wheel_inner_right_rear_state->velocity_)*1/HALFSQRT2;
    current_vel.linear.y = 0.25*WHEELRAD*(-wheel_inner_right_front_state->velocity_+wheel_inner_left_front_state->velocity_+wheel_inner_left_rear_state->velocity_-wheel_inner_right_rear_state->velocity_)*1/HALFSQRT2;
    current_vel.angular.z = -0.25*WHEELRAD/DIS2CENT*(wheel_inner_right_front_state->velocity_+wheel_inner_left_front_state->velocity_+wheel_inner_left_rear_state->velocity_+wheel_inner_right_rear_state->velocity_);

    /*TODO:  Filtering to get better velocity signal, or use transfer function as controller /////////////////////////////////////////////////////////////
       chain.update(vel, vel_filt);
     */

    //compute translational and rotational velocity
    double trans_vel_mag_sq = current_vel.linear.x * current_vel.linear.x + current_vel.linear.y * current_vel.linear.y;
    double rot_vel_mag = fabs(current_vel.angular.z);


    double desired_acc_x, desired_acc_y, desired_acc_phi;
    double error_vel_x, error_vel_y, error_vel_phi;
    double error_pos_x, error_pos_y, error_pos_phi;

    //if zero desired velocity and standing (as good as) still, set stuff to zero
    if (ref_vel.linear.x == 0 && ref_vel.linear.y == 0 && ref_vel.angular.z == 0 && trans_vel_mag_sq < trans_vel_min && rot_vel_mag < rot_vel_min){
        //reset position
        current_pos_x = 0.0;
        current_pos_y = 0.0;
        current_pos_phi = 0.0;

        desired_pos_x = 0.0;
        desired_pos_y = 0.0;
        desired_pos_phi = 0.0;

        //reset acceleration
        desired_acc_x = 0.0;
        desired_acc_y = 0.0;
        desired_acc_phi = 0.0;

        //set zero error
        error_vel_x = 0.0;
        error_vel_y = 0.0;
        error_vel_phi = 0.0;

        error_pos_x = 0.0;
        error_pos_y = 0.0;
        error_pos_phi = 0.0;

    } else {
        //compute desired positions
        desired_pos_x += ref_vel.linear.x * dT;
        desired_pos_y += ref_vel.linear.y * dT;
        desired_pos_phi += ref_vel.angular.z * dT;

        //compute current positions
        current_pos_x += current_vel.linear.x * dT;
        current_pos_y += current_vel.linear.y * dT;
        current_pos_phi += current_vel.angular.z * dT;

        //compute desired accelerations
        desired_acc_x = (ref_vel.linear.x - ref_vel_last_.linear.x) / dT;
        desired_acc_y = (ref_vel.linear.y - ref_vel_last_.linear.y) / dT;
        desired_acc_phi = (ref_vel.angular.z - ref_vel_last_.angular.z) / dT;

        //compute position errors
        error_pos_x = desired_pos_x - current_pos_x;
        error_pos_y = desired_pos_y - current_pos_y;
        error_pos_phi = desired_pos_phi - current_pos_phi;

        //compute velocity errors
        error_vel_x = ref_vel.linear.x - current_vel.linear.x;
        error_vel_y = ref_vel.linear.y - current_vel.linear.y;
        error_vel_phi = ref_vel.angular.z - current_vel.angular.z;
    }

    //compute control effort
    double u_x = mass_x*desired_acc_x + damping_x * ref_vel.linear.x + Kp_x * error_pos_x + Kd_x * error_vel_x;
    double u_y = mass_y*desired_acc_y + damping_y * ref_vel.linear.y + Kp_y * error_pos_y + Kd_y * error_vel_y;
    double u_phi = -(inertia_phi * desired_acc_phi + damping_phi * ref_vel.angular.z + Kp_phi * error_pos_phi + Kd_phi * error_vel_phi);

    //limit control effort
    u_x = std::max<double>(std::min<double>(u_x,u_x_max),-u_x_max);
    u_y = std::max<double>(std::min<double>(u_y,u_y_max),-u_y_max);
    u_phi = std::max<double>(std::min<double>(u_phi,u_phi_max),-u_phi_max);

    //convert the control effort to the effort on the wheel torques
    wheel_inner_left_front_state->commanded_effort_ = 0.25*WHEELRAD*(+1/HALFSQRT2*u_x + 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi);
    wheel_inner_right_front_state->commanded_effort_ = 0.25*WHEELRAD*(1/HALFSQRT2*u_x - 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi);
    wheel_inner_left_rear_state->commanded_effort_ = 0.25*WHEELRAD*(-1/HALFSQRT2*u_x + 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi);
    wheel_inner_right_rear_state->commanded_effort_ = 0.25*WHEELRAD*(-1/HALFSQRT2*u_x - 1/HALFSQRT2*u_y + 1/DIS2CENT*u_phi);

    //make current values last values
    time_of_last_cycle_ = robot_->getTime();

    ref_vel_last_ = ref_vel;
}


void BaseController::stopping() {

}

void BaseController::VelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
    geometry_msgs::Twist base_vel_msg_ = *msg;

    cmd_received_timestamp_ = robot_->getTime();

    ROS_DEBUG("BaseController:: command received: %f %f %f",base_vel_msg_.linear.x,base_vel_msg_.linear.y,base_vel_msg_.angular.z);
    ROS_DEBUG("BaseController:: command current: %f %f %f", ref_vel_last_.linear.x,ref_vel_last_.linear.y,ref_vel_last_.angular.z);

    double vel_mag = sqrt(base_vel_msg_.linear.x * base_vel_msg_.linear.x + base_vel_msg_.linear.y * base_vel_msg_.linear.y);
    ROS_DEBUG("BaseController:: vel: %f", vel_mag);

    if(vel_mag > EPS)
    {
        //limit translational velocity
        double clamped_vel_mag = std::max<double>(std::min<double>(vel_mag, max_translational_velocity_), -max_translational_velocity_);
        ROS_DEBUG("BaseController:: clamped vel: %f", clamped_vel_mag);

        cmd_vel_.linear.x = base_vel_msg_.linear.x * clamped_vel_mag / vel_mag;
        cmd_vel_.linear.y = base_vel_msg_.linear.y * clamped_vel_mag / vel_mag;
    }
    else
    {
        ROS_DEBUG("Basecontroller:: angular velocity is smaller/equal to epsilon; %f", EPS);
        cmd_vel_.linear.x = 0.0;
        cmd_vel_.linear.y = 0.0;
    }

    double ang_mag =(double)base_vel_msg_.angular.z;

    //limit rotational velocity
    cmd_vel_.angular.z = std::max<double>(std::min<double>(ang_mag, max_rotational_velocity_), -max_rotational_velocity_);
}


//linear interpolation, accounting for acceleration limits
geometry_msgs::Twist BaseController::interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT) {
    geometry_msgs::Twist result; //to return
    geometry_msgs::Twist alpha; //multiplier for delta
    geometry_msgs::Twist delta; //difference between start and end
    double max_delta(0); //maximum acceleration allowed


    delta.linear.x = end.linear.x - start.linear.x;
    delta.linear.y = end.linear.y - start.linear.y;
    delta.angular.z = end.angular.z - start.angular.z;

    max_delta = max_rate.linear.x * dT;
    if(fabs(delta.linear.x) <= max_delta || max_delta < EPS)
        alpha.linear.x = 1;
    else
        alpha.linear.x = max_delta / fabs(delta.linear.x);

    max_delta = max_rate.linear.y * dT;
    if(fabs(delta.linear.y) <= max_delta || max_delta < EPS)
        alpha.linear.y = 1;
    else
        alpha.linear.y = max_delta / fabs(delta.linear.y);

    max_delta = max_rate.angular.z * dT;
    if(fabs(delta.angular.z) <= max_delta || max_delta < EPS)
        alpha.angular.z = 1;
    else
        alpha.angular.z = max_delta / fabs(delta.angular.z);

    double alpha_min = std::min<double>(std::min<double>(alpha.linear.x, alpha.linear.y), alpha.angular.z);

    result.linear.x = start.linear.x + alpha_min * (end.linear.x - start.linear.x);
    result.linear.y = start.linear.y + alpha_min * (end.linear.y - start.linear.y);
    result.angular.z = start.angular.z + alpha_min * (end.angular.z - start.angular.z);

    return result;
}