///Calculates from the cartesian velocity the individual wheel velocities
///@param longitudinalVelocity is the forward or backward velocity
///@param transversalVelocity is the sideway velocity
///@param angularVelocity is the rotational velocity around the center of the YouBot
///@param wheelVelocities are the individual wheel velocities
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
}
///Calculates from the cartesian position the wheel positions
///@param longitudinalPosition is the forward or backward position
///@param transversalPosition is the sideway position
///@param orientation is the rotation around the center
///@param wheelPositions are the individual positions of the wheels
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
}
inline
bool
isless BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
const quantity<Unit,Y>& q2)
{
using namespace detail;
return isless BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}
inline quantity<probability_system::probability_unit, decltype(std::declval<X>() + std::declval<Y>())>
operator+(const quantity<unit<probability_system::probability_dimension, System1>, X>& lhs,
const quantity<unit<probability_system::probability_dimension, System2>, Y>& rhs)
{
using type = quantity<probability_system::probability_unit, decltype(std::declval<X>() + std::declval<Y>())>;
return type::from_value(lhs.value() + rhs.value());
}
quantity<R1, R2>
operator-(const quantity<R1, R2>& x, const quantity<R1, R2>& y)
{
typedef quantity<R1, R2> R;
R r;
r.set(x.get() - y.get());
return r;
}
quantity<typename boost::ratio_add<R1, R3>::type, typename boost::ratio_add<R2, R4>::type>
operator*(const quantity<R1, R2>& x, const quantity<R3, R4>& y)
{
typedef quantity<typename boost::ratio_add<R1, R3>::type, typename boost::ratio_add<R2, R4>::type> R;
R r;
r.set(x.get() * y.get());
return r;
}
ScaledProjection::ScaledProjection(
ImagePosition size,
quantity<camera::resolution> x,
quantity<camera::resolution> y )
: size(size),
to_sample( 1.0 / x.value() , 1.0 / y.value() ),
to_image( to_sample.inverse() )
{
}
inline
quantity<Unit, Y>
fmod(const quantity<Unit,Y>& q1, const quantity<Unit,Y>& q2)
{
using std::fmod;
typedef quantity<Unit,Y> quantity_type;
return quantity_type::from_value(fmod(q1.value(), q2.value()));
}
inline
quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>
pow(const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q1,
const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q2)
{
using std::pow;
typedef quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S),Y> quantity_type;
return quantity_type::from_value(pow(q1.value(), q2.value()));
}
inline
bool
isnormal BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
using namespace detail;
return isnormal BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}
inline
int
fpclassify BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
using namespace detail;
return fpclassify BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}
inline
quantity<Unit, Y>
ldexp(const quantity<Unit, Y>& q,const Int& ex)
{
using std::ldexp;
typedef quantity<Unit,Y> quantity_type;
return quantity_type::from_value(ldexp(q.value(), ex));
}
inline
quantity<Unit, Y>
frexp(const quantity<Unit, Y>& q,Int* ex)
{
using std::frexp;
typedef quantity<Unit,Y> quantity_type;
return quantity_type::from_value(frexp(q.value(),ex));
}
inline
quantity<Unit,Y>
trunc BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
using namespace detail;
typedef quantity<Unit,Y> quantity_type;
return quantity_type::from_value(trunc BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}
inline
quantity<Unit,Y> nexttoward BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
const quantity<Unit,Y>& q2)
{
using namespace detail;
typedef quantity<Unit,Y> quantity_type;
return quantity_type::from_value(nexttoward BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}
inline
quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>
log10(const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q)
{
using std::log10;
typedef quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y> quantity_type;
return quantity_type::from_value(log10(q.value()));
}
inline
quantity<Unit, Y>
modf(const quantity<Unit, Y>& q1, quantity<Unit, Y>* q2)
{
using std::modf;
typedef quantity<Unit,Y> quantity_type;
return quantity_type::from_value(modf(q1.value(), &quantity_cast<Y&>(*q2)));
}
inline
typename root_typeof_helper<
quantity<Unit,Y>,
static_rational<2>
>::type
sqrt(const quantity<Unit,Y>& q)
{
using std::sqrt;
typedef typename root_typeof_helper<
quantity<Unit,Y>,
static_rational<2>
>::type quantity_type;
return quantity_type::from_value(sqrt(q.value()));
}
inline
typename add_typeof_helper<
typename multiply_typeof_helper<quantity<Unit1,Y>,
quantity<Unit2,Y> >::type,
quantity<Unit3,Y> >::type
fma BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit1,Y>& q1,
const quantity<Unit2,Y>& q2,
const quantity<Unit3,Y>& q3)
{
using namespace detail;
typedef quantity<Unit1,Y> type1;
typedef quantity<Unit2,Y> type2;
typedef quantity<Unit3,Y> type3;
typedef typename multiply_typeof_helper<type1,type2>::type prod_type;
typedef typename add_typeof_helper<prod_type,type3>::type quantity_type;
return quantity_type::from_value(fma BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value(),q3.value()));
}
inline
typename root_typeof_helper<
typename add_typeof_helper<
typename power_typeof_helper<quantity<Unit,Y>,
static_rational<2> >::type,
typename power_typeof_helper<quantity<Unit,Y>,
static_rational<2> >::type>::type,
static_rational<2> >::type
hypot BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,const quantity<Unit,Y>& q2)
{
using namespace detail;
typedef quantity<Unit,Y> type1;
typedef typename power_typeof_helper<type1,static_rational<2> >::type pow_type;
typedef typename add_typeof_helper<pow_type,pow_type>::type add_type;
typedef typename root_typeof_helper<add_type,static_rational<2> >::type quantity_type;
return quantity_type::from_value(hypot BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}
static quantity<s2::length,Y> convert(const quantity<s1::length,X>& source)
{
return quantity<s2::length,Y>::from_value(2.5*source.value());
}
static quantity<s2::time,Y> convert(const quantity<s1::time,X>& source)
{
return quantity<s2::time,Y>::from_value(0.5*source.value());
}
inline bool operator==(quantity<Unit, Value> const& q, zero_t) {
return q.value() == 0.0;
}
void AutoNavigator::accelerate(quantity<si::velocity, f32> targetSpeed)
{
mAccelerationFactor = targetSpeed.value() / mMaxSpeed.value();
}
std::string to_magnitude( quantity< Dims, T > const & q )
{
std::ostringstream os;
os << q.get( detail::permit<T>() );
return os.str();
}
