bool constraint_samplers::IKConstraintSampler::sample(planning_models::KinematicState::JointStateGroup *jsg, const planning_models::KinematicState &ks, unsigned int max_attempts)
{
// make sure we at least have a chance of sampling using IK; we need at least some kind of constraint
if (!sampling_pose_.position_constraint_ && !sampling_pose_.orientation_constraint_)
return false;
// load an IK solver if we need to
if (!loadIKSolver())
{
kb_.reset();
return false;
}
for (unsigned int a = 0 ; a < max_attempts ; ++a)
{
// sample a point in the constraint region
Eigen::Vector3d point;
Eigen::Quaterniond quat;
if (!samplePose(point, quat, ks, max_attempts))
return false;
geometry_msgs::Pose ik_query;
ik_query.position.x = point.x();
ik_query.position.y = point.y();
ik_query.position.z = point.z();
ik_query.orientation.x = quat.x();
ik_query.orientation.y = quat.y();
ik_query.orientation.z = quat.z();
ik_query.orientation.w = quat.w();
if (callIK(ik_query, ik_timeout_, jsg))
return true;
}
return false;
}
void MyWindow::timeStepping()
{
if (!mSimulating) return;
int numFrames = static_cast<int>(mMovie.size());
if (mController2)
{
numFrames = std::min(numFrames, static_cast<int>(mMovie2.size()));
Eigen::VectorXd qhat2 = samplePose(mMovie2, mTime);// mMovie2[mFrameCount % numFrames].mPose;
mController2->robot()->setPositions(qhat2);
mController2->robot()->computeForwardKinematics(true, true, false);
}
Eigen::VectorXd qhat = samplePose(mMovie, mTime); // mMovie[mFrameCount % numFrames].mPose;
mController->robot()->setPositions(qhat);
mController->robot()->computeForwardKinematics(true, true, false);
mTime = mFrameCount * mController->robot()->getTimeStep();
LOG(INFO) << mFrameCount << ": " << mTime;
mFrameCount++;
}
