bool KDLRobotModel::computeFastIK(
const Eigen::Affine3d& pose,
const RobotState& start,
RobotState& solution)
{
// transform into kinematics frame and convert to kdl
auto* T_map_kinematics = GetLinkTransform(&this->robot_state, m_kinematics_link);
KDL::Frame frame_des;
tf::transformEigenToKDL(T_map_kinematics->inverse() * pose, frame_des);
// seed configuration
for (size_t i = 0; i < start.size(); i++) {
m_jnt_pos_in(i) = start[i];
}
// must be normalized for CartToJntSearch
NormalizeAngles(this, &m_jnt_pos_in);
if (m_ik_solver->CartToJnt(m_jnt_pos_in, frame_des, m_jnt_pos_out) < 0) {
return false;
}
NormalizeAngles(this, &m_jnt_pos_out);
solution.resize(start.size());
for (size_t i = 0; i < solution.size(); ++i) {
solution[i] = m_jnt_pos_out(i);
}
return true;
}
void VPathRenderingData::GetLinkTransforms(hkvMat3* pRotations, hkvVec3* pTranslations, bool bRemoveRoll, bool bReverseOrder) const
{
// Helper to detect link direction reversal when removing roll
bool bDirection = false;
hkvMat3 mReverseRot(hkvNoInitialization);
if (bReverseOrder)
mReverseRot.setFromEulerAngles(0, 0, 180);
else
mReverseRot.setIdentity();
for (unsigned int i = 0; i < GetNumLinks(); ++i)
{
hkvMat3& mRot = pRotations[i];
hkvVec3& vTrans = pTranslations[i];
unsigned int iLink = bReverseOrder ? (GetNumLinks() - i - 1) : i;
GetLinkTransform(iLink, mRot, vTrans);
if (bReverseOrder)
mRot = mRot.multiply(mReverseRot);
if ((i > 0) && bRemoveRoll)
RemoveLinkRoll(pRotations[i - 1], mRot, bDirection);
}
}
bool KDLRobotModel::computeIKSearch(
const Eigen::Affine3d& pose,
const RobotState& start,
RobotState& solution)
{
// transform into kinematics and convert to kdl
auto* T_map_kinematics = GetLinkTransform(&this->robot_state, m_kinematics_link);
KDL::Frame frame_des;
tf::transformEigenToKDL(T_map_kinematics->inverse() * pose, frame_des);
// seed configuration
for (size_t i = 0; i < start.size(); i++) {
m_jnt_pos_in(i) = start[i];
}
// must be normalized for CartToJntSearch
NormalizeAngles(this, &m_jnt_pos_in);
auto initial_guess = m_jnt_pos_in(m_free_angle);
auto start_time = smpl::clock::now();
auto loop_time = 0.0;
auto count = 0;
auto num_positive_increments =
(int)((GetSolverMinPosition(this, m_free_angle) - initial_guess) /
this->m_search_discretization);
auto num_negative_increments =
(int)((initial_guess - GetSolverMinPosition(this, m_free_angle)) /
this->m_search_discretization);
while (loop_time < this->m_timeout) {
if (m_ik_solver->CartToJnt(m_jnt_pos_in, frame_des, m_jnt_pos_out) >= 0) {
NormalizeAngles(this, &m_jnt_pos_out);
solution.resize(start.size());
for (size_t i = 0; i < solution.size(); ++i) {
solution[i] = m_jnt_pos_out(i);
}
return true;
}
if (!getCount(count, num_positive_increments, -num_negative_increments)) {
return false;
}
m_jnt_pos_in(m_free_angle) = initial_guess + this->m_search_discretization * count;
ROS_DEBUG("%d, %f", count, m_jnt_pos_in(m_free_angle));
loop_time = to_seconds(smpl::clock::now() - start_time);
}
if (loop_time >= this->m_timeout) {
ROS_DEBUG("IK Timed out in %f seconds", this->m_timeout);
return false;
} else {
ROS_DEBUG("No IK solution was found");
return false;
}
return false;
}
