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