void
Metric::clip(::rl::math::Vector& q) const
{
assert(q.size() == this->getDofPosition());
for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
{
::rl::math::Vector qi = q.segment(j, this->joints[i]->getDofPosition()); // TODO
this->joints[i]->clip(qi);
q.segment(j, this->joints[i]->getDofPosition()) = qi; // TODO
}
}
void
Metric::step(const ::rl::math::Vector& q1, const ::rl::math::Vector& dq, ::rl::math::Vector& q2) const
{
assert(q1.size() == this->getDofPosition());
assert(dq.size() == this->getDof());
assert(q2.size() == this->getDofPosition());
for (::std::size_t i = 0, j = 0, k = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), k += this->joints[i]->getDof(), ++i)
{
this->joints[i]->step(
q1.segment(j, this->joints[i]->getDofPosition()),
dq.segment(k, this->joints[i]->getDof()),
q2.segment(j, this->joints[i]->getDofPosition())
);
}
}
::rl::math::Real
Metric::distance(const ::rl::math::Vector& q1, const ::rl::math::Vector& q2) const
{
assert(q1.size() == this->getDofPosition());
assert(q2.size() == this->getDofPosition());
::rl::math::Real d = 0;
for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
{
d += this->joints[i]->transformedDistance(
q1.segment(j, this->joints[i]->getDofPosition()),
q2.segment(j, this->joints[i]->getDofPosition())
);
}
return this->inverseOfTransformedDistance(d);
}
void
Metric::interpolate(const ::rl::math::Vector& q1, const ::rl::math::Vector& q2, const ::rl::math::Real& alpha, ::rl::math::Vector& q) const
{
assert(q1.size() == this->getDofPosition());
assert(q2.size() == this->getDofPosition());
assert(alpha >= 0);
assert(alpha <= 1);
assert(q.size() == this->getDofPosition());
for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
{
this->joints[i]->interpolate(
q1.segment(j, this->joints[i]->getDofPosition()),
q2.segment(j, this->joints[i]->getDofPosition()),
alpha,
q.segment(j, this->joints[i]->getDofPosition())
);
}
}
void
Metric::clamp(::rl::math::Vector& q) const
{
assert(q.size() == this->getDofPosition());
for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
{
this->joints[i]->clamp(q.segment(j, this->joints[i]->getDofPosition()));
}
}
void
Metric::step(const ::rl::math::Vector& q1, const ::rl::math::Vector& qdot, ::rl::math::Vector& q2) const
{
assert(q1.size() == this->getDofPosition());
assert(qdot.size() == this->getDof());
assert(q2.size() == this->getDofPosition());
for (::std::size_t i = 0, j = 0, k = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), k += this->joints[i]->getDof(), ++i)
{
::rl::math::Vector q2i = q2.segment(j, this->joints[i]->getDofPosition()); // TODO
this->joints[i]->step(
q1.segment(j, this->joints[i]->getDofPosition()),
qdot.segment(k, this->joints[i]->getDof()),
q2i
);
q2.segment(j, this->joints[i]->getDofPosition()) = q2i; // TODO
}
}
bool
Metric::isValid(const ::rl::math::Vector& q) const
{
assert(q.size() == this->getDofPosition());
for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
{
if (!this->joints[i]->isValid(q.segment(j, this->joints[i]->getDofPosition())))
{
return false;
}
}
return true;
}
