/*! 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(); }