double EuclidDistHeuristic::computeDistance(
const Affine3& a,
const Affine3& b) const
{
auto sqrd = [](double d) { return d * d; };
Vector3 diff = b.translation() - a.translation();
double dp2 =
m_x_coeff * sqrd(diff.x()) +
m_y_coeff * sqrd(diff.y()) +
m_z_coeff * sqrd(diff.z());
Quaternion qb(b.rotation());
Quaternion qa(a.rotation());
double dot = qa.dot(qb);
if (dot < 0.0) {
qb = Quaternion(-qb.w(), -qb.x(), -qb.y(), -qb.z());
dot = qa.dot(qb);
}
double dr2 = angles::normalize_angle(2.0 * std::acos(dot));
dr2 *= (m_rot_coeff * dr2);
SMPL_DEBUG_NAMED(LOG, "Compute Distance: sqrt(%f + %f)", dp2, dr2);
return std::sqrt(dp2 + dr2);
}
int EuclidDistHeuristic::GetGoalHeuristic(int state_id)
{
if (state_id == planningSpace()->getGoalStateID()) {
return 0;
}
if (m_pose_ext) {
Affine3 p;
if (!m_pose_ext->projectToPose(state_id, p)) {
return 0;
}
auto& goal_pose = planningSpace()->goal().pose;
const double dist = computeDistance(p, goal_pose);
const int h = FIXED_POINT_RATIO * dist;
double Y, P, R;
angles::get_euler_zyx(p.rotation(), Y, P, R);
SMPL_DEBUG_NAMED(LOG, "h(%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f) = %d", p.translation()[0], p.translation()[1], p.translation()[2], Y, P, R, h);
return h;
} else if (m_point_ext) {
Vector3 p;
if (!m_point_ext->projectToPoint(state_id, p)) {
return 0;
}
auto& goal_pose = planningSpace()->goal().pose;
Vector3 gp(goal_pose.translation());
double dist = computeDistance(p, gp);
const int h = FIXED_POINT_RATIO * dist;
SMPL_DEBUG_NAMED(LOG, "h(%d) = %d", state_id, h);
return h;
} else {
return 0;
}
}
void cnoid::savePCD(SgPointSet* pointSet, const std::string& filename, const Affine3& viewpoint)
{
if(!pointSet->hasVertices()){
throw empty_data_error() << error_info_message(_("Empty pointset"));
}
ofstream ofs;
ofs.open(filename.c_str());
ofs <<
"# .PCD v.7 - Point Cloud Data file format\n"
"VERSION .7\n"
"FIELDS x y z\n"
"SIZE 4 4 4\n"
"TYPE F F F\n"
"COUNT 1 1 1\n";
const SgVertexArray& points = *pointSet->vertices();
const int numPoints = points.size();
ofs << "WIDTH " << numPoints << "\n";
ofs << "HEIGHT 1\n";
ofs << "VIEWPOINT ";
Affine3::ConstTranslationPart t = viewpoint.translation();
ofs << t.x() << " " << t.y() << " " << t.z() << " ";
const Quaternion q(viewpoint.rotation());
ofs << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << "\n";
ofs << "POINTS " << numPoints << "\n";
ofs << "DATA ascii\n";
for(int i=0; i < numPoints; ++i){
const Vector3f& p = points[i];
ofs << p.x() << " " << p.y() << " " << p.z() << "\n";
}
ofs.close();
}