void FlipHandsGoalRegion::sample(std::vector<double> &xyz_rpy)
{
    // compute the approach pose so that the gripper is perpendicular to the given direction with a random slant with
    // respect to the horizontal plane

    boost::uniform_real<double> dist(-0.5, 0.5) ;
    Vector3d pos2_offset = Vector3d(planner->x_tol * dist(gen), planner->y_tol * dist(gen), planner->z_tol * dist(gen)) ;

    // compute the pose of the grasping arm (arm 2)

    Vector3d pos2 = pos2_offset + p1.translation() ;

    double d = (p1.translation() - p2.translation()).norm() ;

    Vector3d pos1 = p2.translation() + pos2_offset +
            Vector3d(boost::uniform_real<double>(-0.05, 0.05)(gen), boost::uniform_real<double>(-0.05, 0.05)(gen), boost::uniform_real<double>(0, 0.2)(gen));

    double roll1, pitch1, yaw1, x1, y1, z1 ;

    x1 = pos1.x(), y1 = pos1.y(), z1 = pos1.z() ;
    if ( planner->arm == "r1" )
        rpyFromQuat(lookAt(Vector3d(1, 0, 0)), roll1, pitch1, yaw1) ;
    else
        rpyFromQuat(lookAt(Vector3d(-1, 0, 0)), roll1, pitch1, yaw1) ;


    double roll2, pitch2, yaw2, x2, y2, z2 ;

    x2 = pos2.x() ; y2 = pos2.y() ; z2 = pos2.z() ;
    rpyFromQuat(Quaterniond(p1.rotation()), roll2, pitch2, yaw2) ;

    double pitch = boost::uniform_real<double>(planner->pitch_tol_min, planner->pitch_tol_max)(gen) ;
    double yaw = boost::uniform_real<double>(planner->yaw_tol_min, planner->yaw_tol_max)(gen) ;
    double roll = boost::uniform_real<double>(planner->roll_tol_min, planner->roll_tol_max)(gen) ;

    roll1 += roll ;  yaw1 += yaw ; pitch1 += pitch ;

    if ( planner->arm == "r1" )
    {
        xyz_rpy.push_back(x1) ;  xyz_rpy.push_back(y1) ;   xyz_rpy.push_back(z1) ;
        xyz_rpy.push_back(roll1) ;  xyz_rpy.push_back(pitch1) ; xyz_rpy.push_back(yaw1) ;

        xyz_rpy.push_back(x2) ;  xyz_rpy.push_back(y2) ;   xyz_rpy.push_back(z2) ;
        xyz_rpy.push_back(roll2) ;  xyz_rpy.push_back(pitch2) ; xyz_rpy.push_back(yaw2) ;


    }
    else
    {
        xyz_rpy.push_back(x2) ;  xyz_rpy.push_back(y2) ;   xyz_rpy.push_back(z2) ;
        xyz_rpy.push_back(roll2) ;  xyz_rpy.push_back(pitch2) ; xyz_rpy.push_back(yaw2) ;

        xyz_rpy.push_back(x1) ;  xyz_rpy.push_back(y1) ;   xyz_rpy.push_back(z1) ;
        xyz_rpy.push_back(roll1) ;  xyz_rpy.push_back(pitch1) ; xyz_rpy.push_back(yaw1) ;

    }

}
Exemple #2
0
void drawRedDotsPlane()
{   // Draw the stimulus ( red-dots plane )
    glDisable(GL_COLOR_MATERIAL);
    glDisable(GL_BLEND);
    glDisable(GL_LIGHTING);

    // IMPORTANT Reset the previous status of transformation
    objectActiveTransformation.setIdentity();
    objectActiveTransformation.translation() = projPointEyeRight + translationFactor;
    if ((int)factors["Translation"]==-1 || (int)factors["Translation"]==-2 )
        objectActiveTransformation.linear().setIdentity();
    else
        objectActiveTransformation.linear()	= (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();

    glPushMatrix();     // PUSH MATRIX
    glLoadIdentity();
    glMultMatrixd(objectActiveTransformation.data());

    Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight-eyeCalibration);

    double argslant = acos( cos(toRadians(factors["Slant"]))*(focalDistance-posAlongLineOfSight.z() )/((focalDistance )));
    instantPlaneSlant = toDegrees(argslant);


    switch ( (int) factors["Tilt"] )
    {
    case 0:
        glRotated( instantPlaneSlant ,0,1,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
        glScaled(1/sin(toRadians( -90-factors["Slant"])),1,1);	//backprojection phase
        break;
    case 90:
        glRotated( -instantPlaneSlant ,1,0,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
        glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
        break;
    case 180:
        glRotated( -instantPlaneSlant ,0,1,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
        glScaled(1/sin(toRadians( -90-factors["Slant"] )),1,1); //backprojection phase
        break;
    case 270:
        glRotated( instantPlaneSlant ,1,0,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
        glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
        break;
    }

    glGetDoublev(GL_MODELVIEW_MATRIX,objectActiveTransformation.data());
    BoundChecker stimBoundariesActive(&cam, &redDotsPlane);
    BoundChecker stimBoundariesPassive(&camPassive, &redDotsPlane);

    stimOutside = ( stimBoundariesActive.checkOutside(objectActiveTransformation) || stimBoundariesPassive.checkOutside(objectActiveTransformation));
    stimDrawer.draw();

    glPopMatrix();	// POP MATRIX

}
Exemple #3
0
Affine3d FromODETransformToEigenTransform(const dReal* pos, const dReal* rot)
{
	Affine3d ret;
	Vector3d trans;
	trans << pos[0], pos[1], pos[2];
	ret.translation() = trans;

	Matrix3d rotation = FromODERotationToEigenMatrix(rot);
	ret.linear() = rotation;
	return ret;
}
geometry_msgs::Pose MoveitPlanningInterface::transformEigenAffine3dToPose(Affine3d e) {
	Vector3d Oe;
	Matrix3d Re;
	geometry_msgs::Pose pose;
	Oe = e.translation();
	Re = e.linear();
	
	Quaterniond q(Re); // convert rotation matrix Re to a quaternion, q
	pose.position.x = Oe(0);
	pose.position.y = Oe(1);
	pose.position.z = Oe(2);
	
	pose.orientation.x = q.x();
	pose.orientation.y = q.y();
	pose.orientation.z = q.z();
	pose.orientation.w = q.w();
	
	return pose;
}
void drawFixation()
{
	switch ( headCalibrationDone )
	{   
	case 1:
		// Fixed stimulus
		glColor3fv(glWhite);
		glDisable(GL_BLEND);
		glPointSize(5);
		glBegin(GL_POINTS);
		glVertex3d(0,0,focalDistance);
		glEnd();
		glPointSize(1);
		break;
	case 2:
		// Fixed stimulus + projected points
		glColor3fv(glWhite);
		glDisable(GL_BLEND);
		glPointSize(5);
		glBegin(GL_POINTS);
		glVertex3d(0,0,focalDistance);
		glColor3fv(glRed);
		glVertex3dv(projPointEyeRight.data());
		glColor3fv(glBlue);
		glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
		glEnd();
		glPointSize(1);
	
		// Draw the calibration circle
		glColor3fv(glWhite);
		break;
	
	case 3:
	{
	// DRAW THE FIXATION POINT 
	double eyeToCenterAngleX= toDegrees(atan(eyeRight.x()/(-focalDistance-eyeRight.z()) ));
	double eyeToCenterAngleY= toDegrees(atan(eyeRight.y()/(-focalDistance-eyeRight.z()) ));
	double projPointAngleX = toDegrees( atan( (projPointEyeRight.x()-eyeRight.x())/abs(projPointEyeRight.z())));
	double maxAllowedTranslationYaw = str2num<double>(parameters.find("MaxAllowedTranslationYaw"));


	Vector3d stimulusCenter(0,0,0);
	Matrix3d objrotation = Matrix3d::Identity();
	// IMPORTANT Reset the previous status of transformation
	objectActiveTransformation.setIdentity();

	switch ( (int) factors["Rotation"] )
	{
	case 2:
	{
		objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
		*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
		instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
		stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
		objectActiveTransformation.linear()=objrotation;
	}
	break;
	case 1:
	{
		objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
		*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
		instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
		stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
		objectActiveTransformation.linear()=objrotation;
	}
	break;
	case 0:
	{
		objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
		*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
		//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
		instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )*factors["RotationSpeed"]+factors["Slant"];
		stimulusCenter  = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
		objectActiveTransformation.linear() = objrotation;
	}
	break;
	}
	objectActiveTransformation.translation() = stimulusCenter;
	Vector3d fixationPointTmp = objectActiveTransformation.translation();

	glPushMatrix();
	glTranslated(fixationPointTmp.x(),fixationPointTmp.y(),fixationPointTmp.z());
	glutSolidSphere(1,10,10);
	glPopMatrix();
	break;	
	}
}
}
void drawRedDotsPlane()
{   // Draw the stimulus ( red-dots plane )
	glDisable(GL_COLOR_MATERIAL);
	glDisable(GL_BLEND);
	glDisable(GL_LIGHTING);

	Matrix3d objrotation ;
	Vector3d stimulusCenter;
    
	double instantPlaneSlant=0; 
	
	// IMPORTANT Reset the previous status of transformation
	objectActiveTransformation.setIdentity();

	switch ( (int) factors["Rotation"] )
	{
	case 2:
	{
		objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
		*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
		instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
		stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
		objectActiveTransformation.linear()=objrotation;
	}
	break;
	case 1:
	{
		objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
		*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
		instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
		stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
		objectActiveTransformation.linear()=objrotation;
	}
	break;
	case 0:
	{
		objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
		*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
		//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
		instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )+factors["Slant"];
		stimulusCenter  = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
		objectActiveTransformation.linear() = objrotation;
	}
	break;

	}
	objectActiveTransformation.translation() = stimulusCenter;
	
	//cerr << instantPlaneSlant << endl;

	glPushMatrix();     // PUSH MATRIX
	glLoadIdentity();
	glMultMatrixd(objectActiveTransformation.data());
	
	switch ( (int) factors["Tilt"] )
    {
    case 0:
        glRotated( instantPlaneSlant ,0,1,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
        glScaled(1/sin(toRadians( -90-factors["Slant"])),1,1);	//backprojection phase
        break;
    case 90:
        glRotated( -instantPlaneSlant ,1,0,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
        glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
        break;
    case 180:
        glRotated( -instantPlaneSlant ,0,1,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
        glScaled(1/sin(toRadians( -90-factors["Slant"] )),1,1); //backprojection phase
        break;
    case 270:
        glRotated( instantPlaneSlant ,1,0,0);
        //objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
        glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
        break;
    }
	stimDrawer.draw();

	glPopMatrix();	// POP MATRIX

}
 const base::Affine3d getTransform() const
 {
     Affine3d trans (this->orientation);
     trans.translation() = this->translation;
     return trans;
 }