differential_drive.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Includes
00011 *****************************************************************************/
00012 
00013 #include <ecl/linear_algebra.hpp>
00014 #include <ecl/geometry/angle.hpp>
00015 #include <ecl/geometry/legacy_pose2d.hpp>
00016 #include "../../../include/ecl/mobile_robot/kinematics/differential_drive.hpp"
00017 
00018 /*****************************************************************************
00019 ** Namespaces
00020 *****************************************************************************/
00021 
00022 namespace ecl {
00023 namespace mobile_robot {
00024 
00025 /*****************************************************************************
00026 ** Using
00027 *****************************************************************************/
00028 
00029 using ecl::Angle;
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 ecl::LegacyPose2D<double> DifferentialDriveKinematics::forward(const double &dleft, const double& dright) const {
00039 
00040   LegacyPose2D<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::LegacyPose2D<double> DifferentialDriveKinematics::forwardWithPlatformVelocity(const double & linearVelocity, const double & angularVelocity ) const
00051 {
00052   ecl::LegacyPose2D<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 ecl::LegacyPose2D<double> &a,
00071                                                      const ecl::LegacyPose2D<double> &b)
00072 {
00073   Vector2d diff;
00074   diff << b.x() - a.x(), b.y() - a.y();
00075 // This rotation doesn't affect the magnitude of ds,
00076 // but it so the sign can work out properly
00077   Vector2d dr = a.rotationMatrix().inverse()*diff;
00078   double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1));
00079   if ( dr(0) < 0.0 ) {
00080     ds = -ds;
00081   }
00082   double dtheta = b.heading() - a.heading(); // difference in headings
00083   ecl::wrap_angle(dtheta);
00084 
00085 // diff << ds, dtheta;
00086   return (diff << ds, dtheta).finished();
00087 }
00088 
00089 // Depracated
00090 
00091 Vector2d DifferentialDriveKinematics::Inverse(const Vector3d &a, const Vector3d &b) {
00092         Vector2d diff;
00093         Vector2d dxy;
00094         ecl::LegacyPose2D<double> posea(a[2], a.segment<2>(0));
00095         ecl::LegacyPose2D<double> poseb(b[2], b.segment<2>(0));
00096         dxy << poseb.x() - posea.x(), poseb.y() - posea.y();
00097 // This rotation doesn't affect the magnitude of ds,
00098 // but it so the sign can work out properly
00099         Rotation2D<double> rotation(-posea.heading());
00100         Vector2d dr = rotation*dxy;
00101 // New LegacyPose2D class can extract the rotation matrix directly.
00102 //      Vector2d dr = a.rotationMatrix().inverse()*dxy;
00103         double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1));
00104         if ( dr(0) < 0.0 ) {
00105                 ds = -ds;
00106         }
00107         double dtheta = b[2] - a[2]; // difference in headings
00108         ecl::wrap_angle(dtheta);
00109 
00110         diff << ds, dtheta;
00111         return diff;
00112 }
00113 
00114 } // namespace mobile_robot
00115 } // namespace ecl


ecl_mobile_robot
Author(s): Daniel Stonier
autogenerated on Thu Jun 6 2019 21:17:06