void
LeuzeRs4::getDistances(::rl::math::Vector& distances) const
{
assert(this->isConnected());
assert(distances.size() >= this->getDistancesCount());
const uint8_t* ptr;
if (this->data[3] & 2)
{
ptr = this->data + 5;
if ((this->data[3] & 3) && (this->data[4] & 128))
{
ptr = this->data + 6;
}
}
else
{
ptr = this->data + 4;
}
const uint8_t& number1 = get(ptr);
get(ptr);//bool motion = get(ptr) & 64 ? true : false;
const uint8_t& number2 = get(ptr);
get(ptr);
const uint8_t& number3 = get(ptr);
get(ptr);
const uint8_t& number4 = get(ptr);
get(ptr);
/*int number = */hostEndianDoubleWord(hostEndianWord(number1, number2), hostEndianWord(number3, number4));
int step = get(ptr);
const uint8_t& startHigh = get(ptr);
const uint8_t& startLow = get(ptr);
int start = hostEndianWord(startHigh, startLow);
const uint8_t& stopHigh = get(ptr);
const uint8_t& stopLow = get(ptr);
int stop = hostEndianWord(stopHigh, stopLow);
for (int i = 0; i < (stop - start) / step + 1; ++i)
{
const uint8_t& dataHigh = get(ptr);
const uint8_t& dataLow = get(ptr);
distances(distances.size() - 1 - i) = hostEndianWord(dataHigh, dataLow & 254) / 1000.0f;
}
}
void
Model::setPosition(const ::rl::math::Vector& q)
{
if (NULL != this->kin)
{
assert(q.size() == this->kin->getDof());
this->kin->setPosition(q);
}
else
{
assert(q.size() == this->mdl->getDof());
this->mdl->setPosition(q);
}
}
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())
);
}
}
void
SickS300::getDistances(::rl::math::Vector& distances) const
{
assert(this->isConnected());
assert(distances.size() >= this->getDistancesCount());
::rl::math::Real scale = 0.01f;
for (::std::size_t i = 0; i < this->getDistancesCount(); ++i)
{
uint16_t value = Endian::hostEndianWord(this->data[24 + 1 + i * 2], this->data[24 + 0 + i * 2]) & 0x1FFF;
switch (this->data[24 + 1 + i * 2] & 224)
{
case 128:
::std::cerr << "Measured value detected within warning field" << ::std::endl;
case 64:
::std::cerr << "Measured value detected within protective field" << ::std::endl;
case 32:
::std::cerr << "Glare (dazzling) detected" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
default:
distances(i) = value * scale;
break;
}
}
}
DamaSupportSurface::DamaSupportSurface(::std::string _name, ::rl::math::Real _height, const ::rl::math::Vector& _min, const ::rl::math::Vector& _max)
{
this->name = _name;
this->height = _height;
this->dim = _max.size();
this->min = _min;
this->max = _max;
}
::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
Jr3::getTorques(::rl::math::Vector& torques) const
{
assert(torques.size() >= 3);
for (::std::size_t i = 3; i < 6; ++i)
{
torques(i) = (this->values[i] - this->zeroes[i]) * 1000;
}
}
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
Jr3::getForcesTorques(::rl::math::Vector& forcesTorques) const
{
assert(forcesTorques.size() >= 6);
for (::std::size_t i = 0; i < 6; ++i)
{
forcesTorques(i) = (this->values[i] - this->zeroes[i]) * 1000;
}
}
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::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
}
}
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
Sampler::generateCollisionFree(::rl::math::Vector& q)
{
assert(q.size() == this->model->getDof());
do
{
this->generate(q);
this->model->setPosition(q);
this->model->updateFrames();
}
while (this->model->isColliding());
}
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;
}
void
Coach::setJointPosition(const ::rl::math::Vector& q)
{
assert(this->getDof() >= q.size());
this->text.clear();
this->text.str("");
this->text << 2;
this->text << " " << this->i;
for (::std::size_t i = 0; i < this->getDof(); ++i)
{
this->text << " " << q(i);
}
this->text << ::std::endl;
}
void
MitsubishiH7::setJointPosition(const ::rl::math::Vector& q)
{
assert(q.size() >= this->getDof());
this->out.dat.jnt.j1 = 0;
this->out.dat.jnt.j2 = 0;
this->out.dat.jnt.j3 = 0;
this->out.dat.jnt.j4 = 0;
this->out.dat.jnt.j5 = 0;
this->out.dat.jnt.j6 = 0;
this->out.dat.jnt.j7 = 0;
this->out.dat.jnt.j8 = 0;
switch (this->getDof())
{
case 8:
this->out.dat.jnt.j8 = static_cast<float>(q(7));
case 7:
this->out.dat.jnt.j7 = static_cast<float>(q(6));
case 6:
this->out.dat.jnt.j6 = static_cast<float>(q(5));
case 5:
this->out.dat.jnt.j5 = static_cast<float>(q(4));
case 4:
this->out.dat.jnt.j4 = static_cast<float>(q(3));
case 3:
this->out.dat.jnt.j3 = static_cast<float>(q(2));
case 2:
this->out.dat.jnt.j2 = static_cast<float>(q(1));
case 1:
this->out.dat.jnt.j1 = static_cast<float>(q(0));
default:
break;
}
this->out.command = MXT_COMMAND_MOVE;
this->out.sendType = MXT_TYPE_JOINT;
}
void
SickLms200::getDistances(::rl::math::Vector& distances) const
{
assert(this->isConnected());
assert(distances.size() >= this->getDistancesCount());
if (this->data[6] & 32)
{
throw DeviceException("partial scan");
}
::rl::math::Real scale;
switch (this->data[6] & 192)
{
case 0:
scale = 0.01f;
break;
case 64:
scale = 0.001f;
break;
default:
throw DeviceException("unknown scale");
break;
}
uint16_t count = Endian::hostEndianWord(this->data[6] & 11, this->data[5]);
uint8_t mask;
switch (this->configuration)
{
case 0x00:
case 0x01:
case 0x02:
mask = 0x1F;
break;
case 0x03:
case 0x04:
case 0x05:
mask = 0x3F;
break;
case 0x06:
mask = 0x7F;
break;
default:
mask = 0x00;
break;
}
uint16_t value;
for (::std::size_t i = 0; i < count; ++i)
{
value = Endian::hostEndianWord(this->data[8 + i * 2] & mask, this->data[7 + i * 2]);
switch (this->configuration)
{
case 0x00:
case 0x01:
case 0x02:
switch (value)
{
case 0x1FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
case 0x1FFE:
::std::cerr << "Dazzling" << ::std::endl;
case 0x1FFD:
::std::cerr << "Operation overflow" << ::std::endl;
case 0x1FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
case 0x1FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
case 0x1FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
break;
default:
distances(i) = value;
distances(i) *= scale;
break;
}
break;
case 0x03:
case 0x04:
case 0x05:
switch (value)
{
case 0x3FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
case 0x3FFE:
::std::cerr << "Dazzling" << ::std::endl;
case 0x3FFD:
::std::cerr << "Operation overflow" << ::std::endl;
case 0x3FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
case 0x3FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
case 0x3FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
break;
default:
distances(i) = value;
distances(i) *= scale;
break;
}
break;
case 0x06:
switch (value)
{
case 0x7FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
case 0x7FFE:
::std::cerr << "Dazzling" << ::std::endl;
case 0x7FFD:
::std::cerr << "Operation overflow" << ::std::endl;
case 0x7FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
case 0x7FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
break;
case 0x7FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
break;
default:
distances(i) = value;
distances(i) *= scale;
break;
}
default:
distances(i) = value;
distances(i) *= scale;
break;
}
}
}