FourSwedishWheelOmniBaseKinematic.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public 
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/
#include <youbot_driver/base-kinematic/FourSwedishWheelOmniBaseKinematic.hpp>
namespace youbot
{

FourSwedishWheelOmniBaseKinematic::FourSwedishWheelOmniBaseKinematic()
{
  // Bouml preserved body begin 000513F1
  this->lastWheelPositionInitialized = false;
  // Bouml preserved body end 000513F1
}

FourSwedishWheelOmniBaseKinematic::~FourSwedishWheelOmniBaseKinematic()
{
  // Bouml preserved body begin 00051471
  // Bouml preserved body end 00051471
}

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)
{
  // Bouml preserved body begin 0004C071
  quantity<angular_velocity> RadPerSec_FromX;
  quantity<angular_velocity> RadPerSec_FromY;
  quantity<angular_velocity> RadPerSec_FromTheta;
  wheelVelocities.assign(4, RadPerSec_FromX);

  if (config.wheelRadius.value() == 0 || config.rotationRatio == 0 || config.slideRatio == 0)
  {
    throw std::out_of_range("The wheelRadius, RotationRatio or the SlideRatio are not allowed to be zero");
  }

  // RadPerSec_FromX = longitudinalVelocity / config.wheelRadius;
  RadPerSec_FromX = longitudinalVelocity.value() / config.wheelRadius.value() * radian_per_second;
  RadPerSec_FromY = transversalVelocity.value() / (config.wheelRadius.value() * config.slideRatio) * radian_per_second;

  // Calculate Rotation Component
  RadPerSec_FromTheta = ((config.lengthBetweenFrontAndRearWheels + config.lengthBetweenFrontWheels)
      / (2.0 * config.wheelRadius)) * angularVelocity;

  wheelVelocities[0] = -RadPerSec_FromX + RadPerSec_FromY + RadPerSec_FromTheta;
  wheelVelocities[1] = RadPerSec_FromX + RadPerSec_FromY + RadPerSec_FromTheta;
  wheelVelocities[2] = -RadPerSec_FromX - RadPerSec_FromY + RadPerSec_FromTheta;
  wheelVelocities[3] = RadPerSec_FromX - RadPerSec_FromY + RadPerSec_FromTheta;

  return;

  // Bouml preserved body end 0004C071
}

void FourSwedishWheelOmniBaseKinematic::wheelVelocitiesToCartesianVelocity(
    const std::vector<quantity<angular_velocity> >& wheelVelocities, quantity<si::velocity>& longitudinalVelocity,
    quantity<si::velocity>& transversalVelocity, quantity<angular_velocity>& angularVelocity)
{
  // Bouml preserved body begin 0004C0F1

  if (wheelVelocities.size() < 4)
    throw std::out_of_range("To less wheel velocities");

  if (config.lengthBetweenFrontAndRearWheels.value() == 0 || config.lengthBetweenFrontWheels.value() == 0)
  {
    throw std::out_of_range(
        "The lengthBetweenFrontAndRearWheels or the lengthBetweenFrontWheels are not allowed to be zero");
  }

  quantity<si::length> wheel_radius_per4 = config.wheelRadius / 4.0;

  quantity<si::length> geom_factor = (config.lengthBetweenFrontAndRearWheels / 2.0)
      + (config.lengthBetweenFrontWheels / 2.0);
  //now convert this to a vx,vy,vtheta
  longitudinalVelocity = (-wheelVelocities[0] + wheelVelocities[1] - wheelVelocities[2] + wheelVelocities[3]).value()
      * wheel_radius_per4.value() * meter_per_second;
  transversalVelocity = (wheelVelocities[0] + wheelVelocities[1] - wheelVelocities[2] - wheelVelocities[3]).value()
      * wheel_radius_per4.value() * meter_per_second;
  angularVelocity = (wheelVelocities[0] + wheelVelocities[1] + wheelVelocities[2] + wheelVelocities[3])
      * (wheel_radius_per4 / geom_factor).value();

  return;
  // Bouml preserved body end 0004C0F1
}

void FourSwedishWheelOmniBaseKinematic::wheelPositionsToCartesianPosition(
    const std::vector<quantity<plane_angle> >& wheelPositions, quantity<si::length>& longitudinalPosition,
    quantity<si::length>& transversalPosition, quantity<plane_angle>& orientation)
{
  // Bouml preserved body begin 00051371

  if (wheelPositions.size() < 4)
    throw std::out_of_range("To less wheel positions");

  if (config.lengthBetweenFrontAndRearWheels.value() == 0 || config.lengthBetweenFrontWheels.value() == 0)
  {
    throw std::out_of_range(
        "The lengthBetweenFrontAndRearWheels or the lengthBetweenFrontWheels are not allowed to be zero");
  }

  if (this->lastWheelPositionInitialized == false)
  {
    lastWheelPositions = wheelPositions;
    longitudinalPos = 0 * meter;
    transversalPos = 0 * meter;
    angle = 0 * radian;
    this->lastWheelPositionInitialized = true;
  }

  quantity<si::length> deltaLongitudinalPos;
  quantity<si::length> deltaTransversalPos;

  quantity<si::length> wheel_radius_per4 = config.wheelRadius / 4.0;

  quantity<si::length> geom_factor = (config.lengthBetweenFrontAndRearWheels / 2.0)
      + (config.lengthBetweenFrontWheels / 2.0);

  quantity<plane_angle> deltaPositionW1 = (wheelPositions[0] - lastWheelPositions[0]);
  quantity<plane_angle> deltaPositionW2 = (wheelPositions[1] - lastWheelPositions[1]);
  quantity<plane_angle> deltaPositionW3 = (wheelPositions[2] - lastWheelPositions[2]);
  quantity<plane_angle> deltaPositionW4 = (wheelPositions[3] - lastWheelPositions[3]);
  lastWheelPositions[0] = wheelPositions[0];
  lastWheelPositions[1] = wheelPositions[1];
  lastWheelPositions[2] = wheelPositions[2];
  lastWheelPositions[3] = wheelPositions[3];

  deltaLongitudinalPos = (-deltaPositionW1 + deltaPositionW2 - deltaPositionW3 + deltaPositionW4).value()
      * wheel_radius_per4.value() * meter;
  deltaTransversalPos = (deltaPositionW1 + deltaPositionW2 - deltaPositionW3 - deltaPositionW4).value()
      * wheel_radius_per4.value() * meter;
  angle += (deltaPositionW1 + deltaPositionW2 + deltaPositionW3 + deltaPositionW4)
      * (wheel_radius_per4 / geom_factor).value();

  longitudinalPos += deltaLongitudinalPos * cos(angle) - deltaTransversalPos * sin(angle);
  transversalPos += deltaLongitudinalPos * sin(angle) + deltaTransversalPos * cos(angle);

  longitudinalPosition = longitudinalPos;
  transversalPosition = transversalPos;
  orientation = angle;

  return;
  // Bouml preserved body end 00051371
}

void FourSwedishWheelOmniBaseKinematic::cartesianPositionToWheelPositions(
    const quantity<si::length>& longitudinalPosition, const quantity<si::length>& transversalPosition,
    const quantity<plane_angle>& orientation, std::vector<quantity<plane_angle> >& wheelPositions)
{
  // Bouml preserved body begin 000C08F1

  quantity<plane_angle> Rad_FromX;
  quantity<plane_angle> Rad_FromY;
  quantity<plane_angle> Rad_FromTheta;
  wheelPositions.assign(4, Rad_FromX);

  if (config.wheelRadius.value() == 0 || config.rotationRatio == 0 || config.slideRatio == 0)
  {
    throw std::out_of_range("The wheelRadius, RotationRatio or the SlideRatio are not allowed to be zero");
  }

  // RadPerSec_FromX = longitudinalVelocity / config.wheelRadius;
  Rad_FromX = longitudinalPosition.value() / config.wheelRadius.value() * radian;
  Rad_FromY = transversalPosition.value() / (config.wheelRadius.value() * config.slideRatio) * radian;

  // Calculate Rotation Component
  Rad_FromTheta = ((config.lengthBetweenFrontAndRearWheels + config.lengthBetweenFrontWheels)
      / (2.0 * config.wheelRadius)) * orientation;

  wheelPositions[0] = -Rad_FromX + Rad_FromY + Rad_FromTheta;
  wheelPositions[1] = Rad_FromX + Rad_FromY + Rad_FromTheta;
  wheelPositions[2] = -Rad_FromX - Rad_FromY + Rad_FromTheta;
  wheelPositions[3] = Rad_FromX - Rad_FromY + Rad_FromTheta;

  return;

  // Bouml preserved body end 000C08F1
}

void FourSwedishWheelOmniBaseKinematic::setConfiguration(
    const FourSwedishWheelOmniBaseKinematicConfiguration& configuration)
{
  // Bouml preserved body begin 0004C171
  this->config = configuration;
  // Bouml preserved body end 0004C171
}

void FourSwedishWheelOmniBaseKinematic::getConfiguration(
    FourSwedishWheelOmniBaseKinematicConfiguration& configuration) const
{
  // Bouml preserved body begin 0004C1F1
  configuration = this->config;
  // Bouml preserved body end 0004C1F1
}

} // namespace youbot