Commit 27c60fe1 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Implement ConstantCurvature::impl_velocityBound

parent 32304673
......@@ -146,6 +146,10 @@ namespace hpp {
/// Virtual implementation of derivative
virtual void impl_derivative
(vectorOut_t result, const value_type& param, size_type order) const;
/// Virtual implementation of velocity bound
virtual void impl_velocityBound (vectorOut_t bound,
const value_type& param0,
const value_type& param1) const;
/// Virtual implementation of path extraction
virtual PathPtr_t impl_extract (const interval_t& paramInterval) const
......
......@@ -28,6 +28,32 @@
namespace hpp {
namespace core {
namespace steeringMethod {
namespace details {
using std::max;
/// \param t0, t1 angles between \f$ 0 \f$ and \f$ 2 \pi \f$.
value_type absCosineMax (const value_type& t0, const value_type& t1)
{
assert (0 <= t0 && t0 <= t1 && t1 <= 2*M_PI);
if (t1 <= M_PI) return max(fabs(cos(t0)), fabs(cos(t1)));
// M_PI < t1 <= 2*M_PI
if (t0 <= M_PI) return 1.;
// M_PI < t0 <= t1 <= 2*M_PI
return max(fabs(cos(t0)), fabs(cos(t1)));
}
/// \param t0, t1 angles between \f$ 0 \f$ and \f$ 2 \pi \f$.
value_type absSineMax (const value_type& t0, const value_type& t1)
{
assert (0 <= t0 && t0 <= t1 && t1 <= 2*M_PI);
if (t1 <= M_PI_2) return sin(t1);
if (t0 <= M_PI_2) return 1.;
if (t0 >= 3*M_PI_2) return - sin(t0);
// M_PI_2 < t0 < 3*M_PI_2
if (t1 >= 3*M_PI_2) return 1.;
return max(sin(t0), sin(t1));
}
}
ConstantCurvaturePtr_t ConstantCurvature::create
(const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
......@@ -261,6 +287,71 @@ namespace hpp {
}
}
void ConstantCurvature::impl_velocityBound
(vectorOut_t bound, const value_type& param0, const value_type& param1) const
{
// Does a linear interpolation on all the joints.
if (paramRange ().first == paramRange ().second) {
bound.setZero();
return;
}
const value_type L = paramLength();
assert (L > 0);
pinocchio::difference <pinocchio::RnxSOnLieGroupMap>
(robot_, end_, initial_, bound);
bound.noalias() = bound.cwiseAbs() / L;
value_type alpha (fabs(curveLength_) / L);
// Compute max of
// dx = alpha * (c0 * c - s0 * s);
// dy = alpha * (c0 * s + s0 * c);
// dtheta = alpha * curvature_;
// onto [ param0, param1 ]
value_type Mx = std::numeric_limits<value_type>::quiet_NaN(),
My = std::numeric_limits<value_type>::quiet_NaN();
if (curvature_ == 0) {
value_type t0 = curvature_ * curveLength_ * (param0 - paramRange().first) / L,
t1 = curvature_ * curveLength_ * (param1 - paramRange().first) / L;
if (t0 > t1) std::swap (t0, t1);
if (t1-t0 >= M_PI)
Mx = My = 1;
else {
// Compute thetaI
const value_type c0 (initial_ [rzId_ + 0]), s0 (initial_ [rzId_ + 1]);
const value_type tI (atan2 (s0, c0));
t0 += tI;
t1 += tI;
// max(|c0 * c - s0 * s|) = max(|cos(t)|) for t in [t0, t1]
// max(|c0 * s + s0 * c|) = max(|sin(t)|) for t in [t0, t1]
value_type kd = std::floor(t0 * M_1_PI/2) * 2*M_PI;
t0 -= kd;
t1 -= kd;
assert (0 <= t0 && t0 <= t1 && t1 <= 2*M_PI);
Mx = details::absCosineMax (t0, t1);
My = details::absSineMax (t0, t1);
}
} else {
Mx = 1.;
My = 0.;
}
assert (Mx >= 0. && My >= 0.);
assert (Mx <= 1. && My <= 1.);
bound [dxyId_ + 0] = alpha * Mx;
bound [dxyId_ + 1] = alpha * My;
bound [drzId_] = alpha * std::fabs (curvature_);
// Set wheel joint velocities
for (std::vector<Wheels_t>::const_iterator w = wheels_.begin ();
w < wheels_.end (); ++w) {
bound [w->j->rankInVelocity ()] = 0;
}
}
PathPtr_t ConstantCurvature::impl_extract
(const interval_t& paramInterval) const throw (projection_error)
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment