Exemplo n.º 1
0
/*! 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;
}
Exemplo n.º 2
0
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();
}
Exemplo n.º 3
0
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();
}
Exemplo n.º 4
0
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();
}
Exemplo n.º 5
0
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();
}