FourSwedishWheelOmniBaseKinematic.cpp
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #include "base-kinematic/FourSwedishWheelOmniBaseKinematic.hpp"
00052 namespace youbot {
00053
00054 FourSwedishWheelOmniBaseKinematic::FourSwedishWheelOmniBaseKinematic() {
00055
00056 this->lastWheelPositionInitialized = false;
00057
00058 }
00059
00060 FourSwedishWheelOmniBaseKinematic::~FourSwedishWheelOmniBaseKinematic() {
00061
00062
00063 }
00064
00065
00066
00067
00068
00069
00070 void FourSwedishWheelOmniBaseKinematic::cartesianVelocityToWheelVelocities(const quantity<si::velocity>& longitudinalVelocity, const quantity<si::velocity>& transversalVelocity, const quantity<si::angular_velocity>& angularVelocity, std::vector<quantity<angular_velocity> >& wheelVelocities) {
00071
00072 quantity<angular_velocity> RadPerSec_FromX;
00073 quantity<angular_velocity> RadPerSec_FromY;
00074 quantity<angular_velocity> RadPerSec_FromTheta;
00075 wheelVelocities.assign(4, RadPerSec_FromX);
00076
00077 if (config.wheelRadius.value() == 0 || config.rotationRatio == 0 || config.slideRatio == 0) {
00078 throw std::out_of_range("The wheelRadius, RotationRatio or the SlideRatio are not allowed to be zero");
00079 }
00080
00081
00082 RadPerSec_FromX = longitudinalVelocity.value() / config.wheelRadius.value() * radian_per_second;
00083 RadPerSec_FromY = transversalVelocity.value() / (config.wheelRadius.value() * config.slideRatio) * radian_per_second;
00084
00085
00086 RadPerSec_FromTheta = ((config.lengthBetweenFrontAndRearWheels + config.lengthBetweenFrontWheels) / (2.0 * config.wheelRadius)) * angularVelocity;
00087
00088 wheelVelocities[0] = -RadPerSec_FromX + RadPerSec_FromY + RadPerSec_FromTheta;
00089 wheelVelocities[1] = RadPerSec_FromX + RadPerSec_FromY + RadPerSec_FromTheta;
00090 wheelVelocities[2] = -RadPerSec_FromX - RadPerSec_FromY + RadPerSec_FromTheta;
00091 wheelVelocities[3] = RadPerSec_FromX - RadPerSec_FromY + RadPerSec_FromTheta;
00092
00093 return;
00094
00095
00096 }
00097
00098
00099
00100
00101
00102
00103 void FourSwedishWheelOmniBaseKinematic::wheelVelocitiesToCartesianVelocity(const std::vector<quantity<angular_velocity> >& wheelVelocities, quantity<si::velocity>& longitudinalVelocity, quantity<si::velocity>& transversalVelocity, quantity<angular_velocity>& angularVelocity) {
00104
00105
00106 if (wheelVelocities.size() < 4)
00107 throw std::out_of_range("To less wheel velocities");
00108
00109 if (config.lengthBetweenFrontAndRearWheels.value() == 0 || config.lengthBetweenFrontWheels.value() == 0) {
00110 throw std::out_of_range("The lengthBetweenFrontAndRearWheels or the lengthBetweenFrontWheels are not allowed to be zero");
00111 }
00112
00113 quantity<si::length> wheel_radius_per4 = config.wheelRadius / 4.0;
00114
00115 quantity<si::length> geom_factor = (config.lengthBetweenFrontAndRearWheels / 2.0) + (config.lengthBetweenFrontWheels / 2.0);
00116
00117 longitudinalVelocity = (-wheelVelocities[0] + wheelVelocities[1] - wheelVelocities[2] + wheelVelocities[3]).value() * wheel_radius_per4.value() * meter_per_second;
00118 transversalVelocity = (wheelVelocities[0] + wheelVelocities[1] - wheelVelocities[2] - wheelVelocities[3]).value() * wheel_radius_per4.value() * meter_per_second;
00119 angularVelocity = (wheelVelocities[0] + wheelVelocities[1] + wheelVelocities[2] + wheelVelocities[3]) * (wheel_radius_per4 / geom_factor).value();
00120
00121 return;
00122
00123 }
00124
00125
00126
00127
00128
00129
00130 void FourSwedishWheelOmniBaseKinematic::wheelPositionsToCartesianPosition(const std::vector<quantity<plane_angle> >& wheelPositions, quantity<si::length>& longitudinalPosition, quantity<si::length>& transversalPosition, quantity<plane_angle>& orientation) {
00131
00132
00133 if (wheelPositions.size() < 4)
00134 throw std::out_of_range("To less wheel positions");
00135
00136 if (config.lengthBetweenFrontAndRearWheels.value() == 0 || config.lengthBetweenFrontWheels.value() == 0) {
00137 throw std::out_of_range("The lengthBetweenFrontAndRearWheels or the lengthBetweenFrontWheels are not allowed to be zero");
00138 }
00139
00140 if (this->lastWheelPositionInitialized == false) {
00141 lastWheelPositions = wheelPositions;
00142 longitudinalPos = 0 * meter;
00143 transversalPos = 0 * meter;
00144 angle = 0 * radian;
00145 this->lastWheelPositionInitialized = true;
00146 }
00147
00148 quantity<si::length> deltaLongitudinalPos;
00149 quantity<si::length> deltaTransversalPos;
00150
00151 quantity<si::length> wheel_radius_per4 = config.wheelRadius / 4.0;
00152
00153 quantity<si::length> geom_factor = (config.lengthBetweenFrontAndRearWheels / 2.0) + (config.lengthBetweenFrontWheels / 2.0);
00154
00155 quantity<plane_angle> deltaPositionW1 = (wheelPositions[0] - lastWheelPositions[0]);
00156 quantity<plane_angle> deltaPositionW2 = (wheelPositions[1] - lastWheelPositions[1]);
00157 quantity<plane_angle> deltaPositionW3 = (wheelPositions[2] - lastWheelPositions[2]);
00158 quantity<plane_angle> deltaPositionW4 = (wheelPositions[3] - lastWheelPositions[3]);
00159 lastWheelPositions[0] = wheelPositions[0];
00160 lastWheelPositions[1] = wheelPositions[1];
00161 lastWheelPositions[2] = wheelPositions[2];
00162 lastWheelPositions[3] = wheelPositions[3];
00163
00164 deltaLongitudinalPos = (-deltaPositionW1 + deltaPositionW2 - deltaPositionW3 + deltaPositionW4).value() * wheel_radius_per4.value() * meter;
00165 deltaTransversalPos = (deltaPositionW1 + deltaPositionW2 - deltaPositionW3 - deltaPositionW4).value() * wheel_radius_per4.value() * meter;
00166 angle += (deltaPositionW1 + deltaPositionW2 + deltaPositionW3 + deltaPositionW4) * (wheel_radius_per4 / geom_factor).value();
00167
00168 longitudinalPos += deltaLongitudinalPos * cos(angle) - deltaTransversalPos * sin(angle);
00169 transversalPos += deltaLongitudinalPos * sin(angle) + deltaTransversalPos * cos(angle);
00170
00171 longitudinalPosition = longitudinalPos;
00172 transversalPosition = transversalPos;
00173 orientation = angle;
00174
00175 return;
00176
00177 }
00178
00179
00180
00181
00182
00183
00184 void FourSwedishWheelOmniBaseKinematic::cartesianPositionToWheelPositions(const quantity<si::length>& longitudinalPosition, const quantity<si::length>& transversalPosition, const quantity<plane_angle>& orientation, std::vector<quantity<plane_angle> >& wheelPositions) {
00185
00186
00187 quantity<plane_angle> Rad_FromX;
00188 quantity<plane_angle> Rad_FromY;
00189 quantity<plane_angle> Rad_FromTheta;
00190 wheelPositions.assign(4, Rad_FromX);
00191
00192 if (config.wheelRadius.value() == 0 || config.rotationRatio == 0 || config.slideRatio == 0) {
00193 throw std::out_of_range("The wheelRadius, RotationRatio or the SlideRatio are not allowed to be zero");
00194 }
00195
00196
00197 Rad_FromX = longitudinalPosition.value() / config.wheelRadius.value() * radian;
00198 Rad_FromY = transversalPosition.value() / (config.wheelRadius.value() * config.slideRatio) * radian;
00199
00200
00201 Rad_FromTheta = ((config.lengthBetweenFrontAndRearWheels + config.lengthBetweenFrontWheels) / (2.0 * config.wheelRadius)) * orientation;
00202
00203 wheelPositions[0] = -Rad_FromX + Rad_FromY + Rad_FromTheta;
00204 wheelPositions[1] = Rad_FromX + Rad_FromY + Rad_FromTheta;
00205 wheelPositions[2] = -Rad_FromX - Rad_FromY + Rad_FromTheta;
00206 wheelPositions[3] = Rad_FromX - Rad_FromY + Rad_FromTheta;
00207
00208 return;
00209
00210
00211 }
00212
00213 void FourSwedishWheelOmniBaseKinematic::setConfiguration(const FourSwedishWheelOmniBaseKinematicConfiguration& configuration) {
00214
00215 this->config = configuration;
00216
00217 }
00218
00219 void FourSwedishWheelOmniBaseKinematic::getConfiguration(FourSwedishWheelOmniBaseKinematicConfiguration& configuration) const {
00220
00221 configuration = this->config;
00222
00223 }
00224
00225
00226 }