void Arm_Cartesian_Control::checkLimits(
double dt, KDL::JntArray& joint_positions,
KDL::JntArray& jntVel) {
if (upper_joint_limits.size() < arm_chain->getNrOfJoints()
|| lower_joint_limits.size() < arm_chain->getNrOfJoints()) {
std::cout << "No Joint limits defined";
return;
}
double limit_range = 0.1;
jointLimitsReached = false;
for (int i=0; i<jntVel.data.rows(); i++) {
double pos = joint_positions.data(i);
double vel = jntVel.data(i);
double fpos = pos + vel * dt * 2;
double upper_limit = this->upper_joint_limits[i];
double lower_limit = this->lower_joint_limits[i];
double upper_clearance = upper_limit - fpos;
double lower_clearance = lower_limit - fpos;
double limit_vel = vel;
if (fabs(upper_clearance) < limit_range*2 && vel > 0) {
limit_vel *= upper_clearance;
}
if (fabs(lower_clearance) < limit_range*2 && vel < 0) {
limit_vel *= lower_clearance;
}
if (fpos > upper_limit - limit_range && vel > 0) {
limit_vel = 0.0;
std::cout << "Upper limit reached for joint " << i << std::endl;
jointLimitsReached = true;
} else if (fpos < lower_limit + limit_range && vel < 0) {
limit_vel = 0.0;
std::cout << "Lower limit reached for joint " << i << std::endl;
jointLimitsReached = true;
}
jntVel.data(i) = limit_vel;
}
}
