TaskMoveTo.cpp

00001 /*
00002  * TaskMoveTo.cpp
00003  * Copyright (c) 2017, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "TaskMoveTo.h"
00009 
00010 using namespace std;
00011 
00012 const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f;    // default velocity value, given in [m/s]
00013 const float TaskMoveTo::DEFAULT_ZONE = 0.01f;       // default zone value, given in [m]
00014 const float TaskMoveTo::PI = 3.14159265f;           // the constant PI
00015 const float TaskMoveTo::K1 = 2.0f;                  // position controller gain parameter
00016 const float TaskMoveTo::K2 = 0.5f;                  // position controller gain parameter
00017 const float TaskMoveTo::K3 = 0.5f;                  // position controller gain parameter
00018 
00019 /**
00020  * Creates a task object that moves the robot to a given pose.
00021  * @param conroller a reference to the controller object of the robot.
00022  * @param x the x coordinate of the target position, given in [m].
00023  * @param y the y coordinate of the target position, given in [m].
00024  * @param alpha the target orientation, given in [rad].
00025  */
00026 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
00027     
00028     this->x = x;
00029     this->y = y;
00030     this->alpha = alpha;
00031     this->velocity = DEFAULT_VELOCITY;
00032     this->zone = DEFAULT_ZONE;
00033 }
00034 
00035 /**
00036  * Creates a task object that moves the robot to a given pose.
00037  * @param conroller a reference to the controller object of the robot.
00038  * @param x the x coordinate of the target position, given in [m].
00039  * @param y the y coordinate of the target position, given in [m].
00040  * @param alpha the target orientation, given in [rad].
00041  * @param velocity the maximum translational velocity, given in [m/s].
00042  */
00043 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
00044     
00045     this->x = x;
00046     this->y = y;
00047     this->alpha = alpha;
00048     this->velocity = velocity;
00049     this->zone = DEFAULT_ZONE;
00050 }
00051 
00052 /**
00053  * Creates a task object that moves the robot to a given pose.
00054  * @param conroller a reference to the controller object of the robot.
00055  * @param x the x coordinate of the target position, given in [m].
00056  * @param y the y coordinate of the target position, given in [m].
00057  * @param alpha the target orientation, given in [rad].
00058  * @param velocity the maximum translational velocity, given in [m/s].
00059  * @param zone the zone threshold around the target position, given in [m].
00060  */
00061 TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
00062     
00063     this->x = x;
00064     this->y = y;
00065     this->alpha = alpha;
00066     this->velocity = velocity;
00067     this->zone = zone;
00068 }
00069 
00070 /**
00071  * Deletes the task object.
00072  */
00073 TaskMoveTo::~TaskMoveTo() {}
00074 
00075 /**
00076  * This method is called periodically by a task sequencer.
00077  * @param period the period of the task sequencer, given in [s].
00078  * @return the status of this task, i.e. RUNNING or DONE.
00079  */
00080 int TaskMoveTo::run(float period) {
00081     
00082     float translationalVelocity = 0.0f;
00083     float rotationalVelocity = 0.0f;
00084     
00085     float x = controller.getX();
00086     float y = controller.getY();
00087     float alpha = controller.getAlpha();
00088     
00089     float rho = sqrt((this->x-x)*(this->x-x)+(this->y-y)*(this->y-y));
00090     
00091     if (rho > 0.001f) {
00092         
00093         float gamma = atan2(this->y-y, this->x-x)-alpha;
00094         
00095         while (gamma < -PI) gamma += 2.0f*PI;
00096         while (gamma > PI) gamma -= 2.0f*PI;
00097         
00098         float delta = gamma+alpha-this->alpha;
00099         
00100         while (delta < -PI) delta += 2.0f*PI;
00101         while (delta > PI) delta -= 2.0f*PI;
00102         
00103         translationalVelocity = K1*rho*cos(gamma);
00104         translationalVelocity = (translationalVelocity > velocity) ? velocity : (translationalVelocity < -velocity) ? -velocity : translationalVelocity;
00105         rotationalVelocity = (fabs(gamma) > 0.001f) ? K2*gamma+K1*sin(gamma)*cos(gamma)*(gamma+K3*delta)/gamma : 0.0f;
00106     }
00107     
00108     controller.setTranslationalVelocity(translationalVelocity);
00109     controller.setRotationalVelocity(rotationalVelocity);
00110     
00111     return (rho < zone) ? DONE : RUNNING;
00112 }
00113