/*! Only looks at the relative distance between gripper positions for the final grasps.
Returns true if both of the follosing are true:
- translation between gripper locations is less than DISTANCE_THRESHOLD
- rotation angles between gripper locations is less than ANGULAR_THRESHOLD.
Both thresholds are hard-coded in here.
*/
bool GraspClusteringTask::clusterGrasps(const GraspitDBGrasp *g1, const GraspitDBGrasp *g2)
{
//2 cm distance threshold
double DISTANCE_THRESHOLD = 20;
//30 degrees angular threshold
double ANGULAR_THRESHOLD = 0.52;
transf t1 = g1->getFinalGraspPlanningState()->getTotalTran() % g1->getHand()->getApproachTran();
transf t2 = g2->getFinalGraspPlanningState()->getTotalTran() % g2->getHand()->getApproachTran();
vec3 dvec = t1.translation() - t2.translation();
double d = dvec.norm();
if (d > DISTANCE_THRESHOLD) { return false; }
Quaternion qvec = t1.rotation() * t2.rotation().inverse();
vec3 axis; double angle;
Eigen::AngleAxisd aa (qvec);
angle = aa.angle();
axis = aa.axis();
if (angle > M_PI) { angle -= 2 * M_PI; }
if (angle < -M_PI) { angle += 2 * M_PI; }
if (fabs(angle) > ANGULAR_THRESHOLD) { return false; }
return true;
}
void GlutAxes::draw()
{
glDisable(GL_LIGHTING);
glPushMatrix();
glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
Vector3d axesPoints[4];
axesPoints[0] = Vector3d(0.0, 0.0, 0.0);
axesPoints[1] = (length * Vector3d(1.0, 0.0, 0.0));
axesPoints[2] = (length * Vector3d(0.0, 1.0, 0.0));
axesPoints[3] = (length * Vector3d(0.0, 0.0, 1.0));
glLineWidth(width);
glBegin(GL_LINES);
glColor3d(1.0, 0.0, 0.0);
glVertex3d(axesPoints[0].x(), axesPoints[0].y(), axesPoints[0].z());
glVertex3d(axesPoints[1].x(), axesPoints[1].y(), axesPoints[1].z());
glColor3d(0.0, 1.0, 0.0);
glVertex3d(axesPoints[0].x(), axesPoints[0].y(), axesPoints[0].z());
glVertex3d(axesPoints[2].x(), axesPoints[2].y(), axesPoints[2].z());
glColor3d(0.0, 0.0, 1.0);
glVertex3d(axesPoints[0].x(), axesPoints[0].y(), axesPoints[0].z());
glVertex3d(axesPoints[3].x(), axesPoints[3].y(), axesPoints[3].z());
glEnd();
glPopMatrix();
}
void GlutGroup::draw()
{
glPushMatrix();
glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
for (auto it = children.cbegin(); it != children.cend(); ++it)
{
(*it)->draw();
}
glPopMatrix();
}
void GlutSphere::draw()
{
glEnable(GL_LIGHTING);
glPushMatrix();
glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
glColor3d(color.x(), color.y(), color.z());
gluQuadricOrientation(quadratic, GLU_OUTSIDE);
gluSphere(quadratic, radius, 32, 32);
glPopMatrix();
}
void GlutSquare::draw()
{
glEnable(GL_LIGHTING);
glPushMatrix();
glTranslated(pose.translation().x(), pose.translation().y(), pose.translation().z());
Eigen::AngleAxisd angleAxis = Eigen::AngleAxisd(pose.rotation());
glRotated(angleAxis.angle() * 180.0 / M_PI, angleAxis.axis().x(), angleAxis.axis().y(), angleAxis.axis().z());
Vector3d squarePoints[4];
squarePoints[0] = - (halfSize * planeDirectionX) + (halfSize * planeDirectionY);
squarePoints[1] = (halfSize * planeDirectionX) + (halfSize * planeDirectionY);
squarePoints[2] = (halfSize * planeDirectionX) - (halfSize * planeDirectionY);
squarePoints[3] = - (halfSize * planeDirectionX) - (halfSize * planeDirectionY);
Vector3d normal = (squarePoints[1] - squarePoints[0]).cross(squarePoints[2] - squarePoints[0]);
normal.normalize();
glColor3d(color.x(), color.y(), color.z());
glBegin(GL_QUADS);
// Front side
glNormal3d(normal.x(), normal.y(), normal.z());
for (int i = 0; i < 4; ++i)
{
glVertex3d(squarePoints[i].x(), squarePoints[i].y(), squarePoints[i].z());
}
// Back side
glNormal3d(- normal.x(), - normal.y(), - normal.z());
for (int i = 0; i < 4; ++i)
{
glVertex3d(squarePoints[3 - i].x(), squarePoints[3 - i].y(), squarePoints[3 - i].z());
}
glEnd();
glPopMatrix();
}
