/*! 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;
}
Beispiel #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();
}
//! Get inertial (I) to rotating planetocentric (R) reference frame transformtion matrix.
Eigen::Matrix3d getInertialToPlanetocentricFrameTransformationMatrix(
        const double angleFromXItoXR )
{
    // Compute rotation about Z-Axis.
    // Note the sign change, because how angleAxisd is defined.
    Eigen::AngleAxisd eigenRotationObject = Eigen::AngleAxisd( -1.0 * angleFromXItoXR,
                                                               Eigen::Vector3d::UnitZ( ) );

    // Return transformation matrix.
    return eigenRotationObject.toRotationMatrix( );
}
Beispiel #4
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();
}
Beispiel #5
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();
}
Beispiel #6
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();
}
Beispiel #7
0
/**
 * Returns the bounding box, which includes the plane.
 */
RotatedRectangle Plane::getMaxExpansion(){
	if(!rectangleCalculated){
	std::list<std::list<std::pair<Shared_Point,Shared_Point> > > edgesHull = getTransPoints();
	std::vector<cv::Point2f> _points;
	std::list< std::list<std::pair<Shared_Point, Shared_Point> > >::iterator it;
	std::list< std::pair<Shared_Point, Shared_Point> >::iterator it_list;
	for(it=edgesHull.begin(); it!=edgesHull.end(); it++){
		for(it_list=(*it).begin(); it_list!=(*it).end(); it_list++){
			cv::Point2f p((*it_list).first->x, (*it_list).first->z);
			_points.push_back(p);
			//printf("Point: %f,%f\n",p.x,p.y);
		} 
	}
	if(_points.size()==0){
		fprintf(stderr,"Point size is zero so not calculating maximum expansion, edgesHull Size is: %i\n", (int)edgesHull.size());
		return RotatedRectangle();
	}
    
 
	cv::RotatedRect rect = cv::minAreaRect(cv::Mat(_points));

	double width = (rect.size.width>rect.size.height)?rect.size.width:rect.size.height;
	double height = (rect.size.width<rect.size.height)?rect.size.width:rect.size.height;
	double angle = rect.angle;

	//Berchnung von Eckpunkten mit Hile von Trygonometrie und dann die Punkte in die Welt rotieren
	Eigen::Vector3d centerOfMass(this->centerOfMass[0], this->centerOfMass[1], this->centerOfMass[2]);
	Eigen::Vector3d rotationAxis = normalVector.cross(Eigen::Vector3d::UnitY());
	rotationAxis.normalize();
	Eigen::AngleAxisd m = Eigen::AngleAxisd(acos(normalVector[1]), rotationAxis);
	Eigen::Vector3d inertiaAxisXNew = m*inertiaAxisX;
	inertiaAxisXNew.normalize();
	double angle2 = acos(inertiaAxisXNew.dot(Eigen::Vector3d::UnitX()));	
	Eigen::AngleAxisd m2; 
	if(angle2 != 0.0){ 
		Eigen::Vector3d rotationAxis2 = inertiaAxisXNew.cross(Eigen::Vector3d::UnitX());
		rotationAxis2.normalize(); 
		m2 = Eigen::AngleAxisd(angle2, rotationAxis2); 
	}
	if(angle>20){
		angle-=90;
	}
	angle = angle/180.0*M_PI;
	Eigen::Vector3d point[4];
	for(int i=0; i<4; i++){
		point[i].setZero();
	}
	/*
	point[0][0] = centerOfMass[0] + (width/2.0) * cos(angle) + (height/2.0) * sin(angle);
	point[0][2] = centerOfMass[2] + (width/2.0) * sin(angle) + (height/2.0) * cos(angle);

	point[1][0] = centerOfMass[0] + (-width/2.0) * cos(angle) + (height/2.0) * sin(angle);
	point[1][2] = centerOfMass[2] + (-width/2.0) * sin(angle) + (height/2.0) * cos(angle);

	point[2][0] = centerOfMass[0] + (width/2.0) * cos(angle) + (-height/2.0) * sin(angle);
	point[2][2] = centerOfMass[2] + (width/2.0) * sin(angle) + (-height/2.0) * cos(angle);

	point[3][0] = centerOfMass[0] + (-width/2.0) * cos(angle) + (-height/2.0) * sin(angle);
	point[3][2] = centerOfMass[2] + (-width/2.0) * sin(angle) + (-height/2.0) * cos(angle);
	*/
	double angle_tmp = angle;
	angle=-angle;
	double w = height;
	double h = width;

	point[0][0] = (w/2.0) * cos(angle) - (h/2.0) * sin(angle);
	point[0][2] = (w/2.0) * sin(angle) + (h/2.0) * cos(angle);

	point[1][0] = (-w/2.0) * cos(angle) - (h/2.0) * sin(angle);
	point[1][2] = (-w/2.0) * sin(angle) + (h/2.0) * cos(angle);

	point[2][0] = (w/2.0) * cos(angle) - (-h/2.0) * sin(angle);
	point[2][2] = (w/2.0) * sin(angle) + (-h/2.0) * cos(angle);

	point[3][0] = (-w/2.0) * cos(angle) - (-h/2.0) * sin(angle);
	point[3][2] = (-w/2.0) * sin(angle) + (-h/2.0) * cos(angle);

	angle = angle_tmp;

	RotatedRectangle rot_rect(width, height, angle, rect.center.x, rect.center.y);
	for(int i=0; i<4; i++){
		point[i] += Eigen::Vector3d(rect.center.x,0,rect.center.y);
		point[i] = m2.inverse() * point[i];
		point[i] = m.inverse() * point[i];
		point[i] = point[i] + centerOfMass;
		rot_rect.edges[i]=point[i];
	}
	rectangle = rot_rect;
	}
	return rectangle;
}
Beispiel #8
0
/**
 * Calculates a 2D hull by using alpha shapes and returns a list of pairs.
 */
std::list<std::list<std::pair<Shared_Point,Shared_Point> > > Plane::getHull(){
	std::list<std::list<std::pair<Shared_Point,Shared_Point> > > edgesHull;
	std::set<Shared_Point>::iterator it1;
	double angle, angle2;
	Eigen::Vector3d rotationAxis;
	Eigen::Vector3d centerOfMass(this->centerOfMass[0], this->centerOfMass[1], this->centerOfMass[2]);
	rotationAxis = normalVector.cross(Eigen::Vector3d::UnitY());
	rotationAxis.normalize();
	angle = acos(normalVector[1]);
	Eigen::AngleAxisd m = Eigen::AngleAxisd(angle, rotationAxis);

	Eigen::Vector3d inertiaAxisXNew = m*inertiaAxisX;
	inertiaAxisXNew.normalize();
	angle2 = acos(inertiaAxisXNew.dot(Eigen::Vector3d::UnitX()));	
	Eigen::AngleAxisd m2; 
	if(angle2 != 0.0){ //It crashed if the angle is 0.
		Eigen::Vector3d rotationAxis2 = inertiaAxisXNew.cross(Eigen::Vector3d::UnitX());
		rotationAxis2.normalize(); 
		m2 = Eigen::AngleAxisd(angle2, rotationAxis2); 
	}
	edgesHull = getTransPoints(); 
	if(edgesHull.size()==0)
		return edgesHull;
	
	Shared_Point p = edgesHull.begin()->begin()->first;
	Shared_Point p2= edgesHull.begin()->begin()->first;
	std::list< std::list<std::pair<Shared_Point, Shared_Point> > >::iterator it;
	std::list< std::pair<Shared_Point, Shared_Point> >::iterator it_list;
	for(it=edgesHull.begin(); it!=edgesHull.end(); it++){
		for(it_list=(*it).begin(); it_list!=(*it).end(); it_list++){
			if(fabs((*it_list).first->x)>fabs(p->x))
				p=(*it_list).first;
			if(fabs((*it_list).first->z)>fabs(p2->z))
				p2=(*it_list).first;

			if(fabs((*it_list).second->x)>fabs(p->x))
				p=(*it_list).second;
			if(fabs((*it_list).second->z)>fabs(p2->z))
				p2=(*it_list).second;
		}
	}

	std::list< std::list< std::pair<Shared_Point, Shared_Point> > >::iterator it2;
	std::list< std::pair<Shared_Point, Shared_Point> >::iterator it3;
	for(it2=edgesHull.begin(); it2!=edgesHull.end(); it2++){
		for(it3=(*it2).begin(); it3!=(*it2).end(); it3++){
			Eigen::Vector3d p((*it3).first->p[0], (*it3).first->p[1], (*it3).first->p[2]);
			Eigen::Vector3d p1((*it3).second->p[0], (*it3).second->p[1], (*it3).second->p[2]);
			p = m2.inverse() * p;
			p = m.inverse() * p;
			p = p + centerOfMass;
			(*it3).first->p[0]=p[0];
			(*it3).first->p[1]=p[1];
			(*it3).first->p[2]=p[2];
			
			p1 = m2.inverse() * p1;
			p1 = m.inverse() * p1;
			p1 = p1 + centerOfMass;
			(*it3).second->p[0]=p1[0];
			(*it3).second->p[1]=p1[1];
			(*it3).second->p[2]=p1[2];
		}
	}
	return edgesHull;
}