differential_drive.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Includes
00011 *****************************************************************************/
00012 
00013 #include <ecl/geometry/angle.hpp>
00014 #include <ecl/geometry/pose2d.hpp>
00015 #include "../../../include/ecl/mobile_robot/kinematics/differential_drive.hpp"
00016 
00017 /*****************************************************************************
00018 ** Namespaces
00019 *****************************************************************************/
00020 
00021 namespace ecl {
00022 namespace mobile_robot {
00023 
00024 /*****************************************************************************
00025 ** Using
00026 *****************************************************************************/
00027 
00028 using ecl::Angle;
00029 using ecl::Pose2D;
00030 using ecl::linear_algebra::Vector2d;
00031 using ecl::linear_algebra::Vector3d;
00032 using ecl::linear_algebra::Rotation2D;
00033 
00034 /*****************************************************************************
00035 ** Implementation [Kinematics]
00036 *****************************************************************************/
00037 
00038 Pose2D<double> DifferentialDriveKinematics::forward(const double &dleft, const double& dright) const {
00039 
00040         Pose2D<double> pose_update;
00041         double ds = radius*(dleft+dright)/2.0;
00042         double domega = radius*(dright-dleft)/bias;
00043         // Local robot frame of reference has the x axis pointing forward
00044         // Since the pose update is using the robot frame of reference, the y update is zero
00045         pose_update.translation(ds, 0);
00046         pose_update.rotation(domega);
00047         return pose_update;
00048 }
00049 
00050 ecl::Pose2D<double> DifferentialDriveKinematics::forwardWithPlatformVelocity(const double & linearVelocity, const double & angularVelocity ) const
00051 {
00052         Pose2D<double> pose_update;
00053         // Local robot frame of reference has the x axis pointing forward
00054         // Since the pose update is using the robot frame of reference, the y update is zero
00055         pose_update.translation(linearVelocity, 0);
00056         pose_update.rotation(angularVelocity);
00057         return pose_update;
00058 }
00059 
00060 Vector2d DifferentialDriveKinematics::inverse(const double &linear_velocity, const double &angular_velocity) const {
00061 //      radius*(theta_dot_l+theta_dot_r)/2.0 = linear_velocity
00062 //      radius*(theta_dot_r-theta_dot_l)/bias = angular_velocity
00063         Vector2d rates;
00064         double right = (2*linear_velocity+angular_velocity*bias)/(2*radius);
00065         double left = 2*linear_velocity/radius - right;
00066         rates << left, right;
00067         return rates;
00068 }
00069 
00070 Vector2d DifferentialDriveKinematics::PartialInverse(const Pose2D<double> &a, const Pose2D<double> &b) {
00071         Vector2d diff;
00072         diff << b.x() - a.x(), b.y() - a.y();
00073 // This rotation doesn't affect the magnitude of ds,
00074 // but it so the sign can work out properly
00075         Vector2d dr = a.rotationMatrix().inverse()*diff;
00076         double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1));
00077         if ( dr(0) < 0.0 ) {
00078                 ds = -ds;
00079         }
00080         double dtheta = b.heading() - a.heading(); // difference in headings
00081         ecl::wrap_angle(dtheta);
00082 
00083 //      diff << ds, dtheta;
00084         return (diff << ds, dtheta).finished();
00085 }
00086 
00087 // Depracated
00088 
00089 Vector2d DifferentialDriveKinematics::Inverse(const Vector3d &a, const Vector3d &b) {
00090         Vector2d diff;
00091         Vector2d dxy;
00092         Pose2D<double> posea(a[2], a.segment<2>(0));
00093         Pose2D<double> poseb(b[2], b.segment<2>(0));
00094         dxy << poseb.x() - posea.x(), poseb.y() - posea.y();
00095 // This rotation doesn't affect the magnitude of ds,
00096 // but it so the sign can work out properly
00097         Rotation2D<double> rotation(-posea.heading());
00098         Vector2d dr = rotation*dxy;
00099 // New Pose2D class can extract the rotation matrix directly.
00100 //      Vector2d dr = a.rotationMatrix().inverse()*dxy;
00101         double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1));
00102         if ( dr(0) < 0.0 ) {
00103                 ds = -ds;
00104         }
00105         double dtheta = b[2] - a[2]; // difference in headings
00106         ecl::wrap_angle(dtheta);
00107 
00108         diff << ds, dtheta;
00109         return diff;
00110 }
00111 
00112 } // namespace mobile_robot
00113 } // namespace ecl


ecl_mobile_robot
Author(s): Daniel Stonier (d.stonier@gmail.com)
autogenerated on Thu Jan 2 2014 11:12:21