示例#1
0
void WorldRpcInterface::setRot( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		if ((command.size() - n) == 3) {
			double x = command.get(n).asDouble()*M_PI/180.; n++;  //std::cout << x << std::endl; // x position
			double y = command.get(n).asDouble()*M_PI/180.; n++;  //std::cout << y << std::endl; // y position  
			double z = command.get(n).asDouble()*M_PI/180.; n++;  //std::cout << z << std::endl; // z position
			object->setCartesianOrientation( QVector3D(x,y,z) );
			reply.addString("Set rotation (about x,y,z in degrees).");
		} else {
			// replace rotation part of object's T matrix
			QMatrix4x4 rt = object->getT();
			for (int i = 0; i<3; i++) {
				for (int j = 0; j<3; j++) {
					rt(i, j) = command.get(n).asDouble(); n++;
				}
			}
			object->setT(rt);
			reply.addString("Set full rotation matrix.");
		}

	}
}
示例#2
0
void WorldRpcInterface::removeObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	if ( object )
	{ 
		object->kill();
		reply.addString("Removed object");
	}
}
示例#3
0
void WorldRpcInterface::setRTfromSim(const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n) {
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	if ( object ) {
		QMatrix4x4 rt;
		std::string name = object->getName().toStdString();
		if(parseSimRTBottle(name, command, n, rt)) {
			object->setT(rt);
		} else {
			reply.addString("parseSimRTBottle: invalid number of arguments supplied");
		}

	}
}
示例#4
0
void WorldRpcInterface::set( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		double x = command.get(n).asDouble(); n++;  //std::cout << x << std::endl; // x position
		double y = command.get(n).asDouble(); n++;  //std::cout << y << std::endl; // y position  
		double z = command.get(n).asDouble(); n++;  //std::cout << z << std::endl; // z position
		object->setPosition( QVector3D(x,y,z) );
		reply.addString("Set Cartesian position of object.");
	}
}
示例#5
0
void WorldRpcInterface::getState( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		const qreal* T = object->getT().data();
		for ( int i = 0; i < 16; i++ )
		{
			reply.addDouble( T[i] );
		}
		//reply.addString("Set rotation (about x,y,z in degrees) of object.");
	}
}
示例#6
0
文件: objectsoup.cpp 项目: Juxi/iCub
KinematicModel::CompositeObject* ObjectSoup::makeARandomObjectLikeAMothaFucka()
{
	KinematicModel::CompositeObject* obj = new KinematicModel::CompositeObject( theModel.OBSTACLE(), theModel.OBSTACLE() );
	
	// choose a random color for the object
	QColor collidingColor = QColor( qrand()%255, qrand()%255, qrand()%255, 0.5 );
	QColor freeColor = collidingColor;
	freeColor.setAlphaF( 1.0 );
	obj->setCollidingColor(collidingColor);
	obj->setFreeColor(freeColor);
	
	// choose a random number of primitives to append to the object 1 to 5 inclusive
	int numPrimitives =  (qrand() % 6 + 1);
	if (verbose) printf( " Num Primitives: %d\n", numPrimitives);
	KinematicModel::PrimitiveObject* primitive;
	for ( int j=0; j<numPrimitives; ++j )
	{
		// choose a random geometry type for each primitive
		KinematicModel::GeomType geomType =  (KinematicModel::GeomType)( qrand() % ((int)KinematicModel::BOX+1) );
		if (verbose) printf( "  Geometry Type: %d\n", (int)geomType );
		switch (geomType)
		{
			case KinematicModel::SPHERE:	primitive = new KinematicModel::Sphere( (float)qrand()/RAND_MAX/10.0+0.01 );
				break;
			case KinematicModel::CYLINDER:	primitive = new KinematicModel::Cylinder( (float)qrand()/RAND_MAX/10+.01,
																					  (float)2*qrand()/RAND_MAX/10+.01 );
				break;
			case KinematicModel::BOX:		primitive = new KinematicModel::Box( QVector3D((float)qrand()/RAND_MAX/10+.01,
																						   (float)qrand()/RAND_MAX/10+.01,
																						   (float)qrand()/RAND_MAX/10+.01) );
				break;
			default:
				break;
		}
		primitive->setCollidingColor( collidingColor );
		primitive->setFreeColor(freeColor);
		primitive->translate(randomTranslation(0.1));
		primitive->cartesianRotate(randomRotation());
		obj->appendPrimitive(primitive);
	}
	obj->translate(randomTranslation(1.0));
	primitive->cartesianRotate(randomRotation());
	theModel.appendObject(obj);
	return obj;
}
示例#7
0
void WorldRpcInterface::respClass( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n  )
{
	KinematicModel::CompositeObject* object = getObject( command, reply, n );
	
	if ( object )
	{
		//model->clearWorldObject(object);
        model->removeWorldObject(object);

		int type = command.get(n).asVocab();
		QColor collidingColor,freeColor;
		switch (type)
		{
			case VOCAB_OBSTACLE:
				
				freeColor = Qt::blue;
				freeColor = freeColor.lighter();

				collidingColor = freeColor;
                
                freeColor.setAlphaF(0.5);
				collidingColor.setAlphaF(0.5);
				
				object->setResponseClass(model->OBSTACLE());
				object->setFreeColor( freeColor );
				object->setCollidingColor( collidingColor );
				
				reply.addString("Changed object type to 'obstacle'.");
				
				break;
				
			case VOCAB_TARGET:
				
				freeColor = Qt::green;
				freeColor = freeColor.lighter();
				collidingColor = freeColor;
				
				freeColor.setAlphaF(1.0);
				collidingColor.setAlphaF(0.5);
				
				object->setResponseClass(model->TARGET());
				object->setFreeColor( freeColor );
				object->setCollidingColor( collidingColor );
	
				reply.addString("Changed object type to 'target'.");
				
				break;
				
			default:
				
				reply.addString("Unknown definition, use 'obs' or 'tgt'.");
		}
		
		model->appendObject( object );
	}
}
示例#8
0
void WorldRpcInterface::make( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
	int geom = command.get(n).asVocab();
	n++;
	 
	try {
		KinematicModel::CompositeObject* composite = new KinematicModel::CompositeObject( model->OBSTACLE(), model->FIELD() );
		KinematicModel::PrimitiveObject* primitive;
		if ( geom == VOCAB_SPH || geom == VOCAB_SSPH ) {
			
			double r = command.get(n).asDouble(); n++; // radius 
			double px = command.get(n).asDouble(); n++; // x position
			double py = command.get(n).asDouble(); n++; // y position  
			double pz = command.get(n).asDouble(); n++; // z position
			
			primitive = new KinematicModel::Sphere( r );
			composite->appendPrimitive( primitive );
			composite->setPosition( QVector3D(px,py,pz) );
            
            QString name;
			if ( geom == VOCAB_SPH ) { name = "sph" + QString::number(++s); }
			else if ( geom == VOCAB_SSPH ) { name = "ssph" + QString::number(++ss); }
            composite->setName(name);
            //printf("%s\n",name.toLocal8Bit().data());
			reply.addString( name.toLocal8Bit().data() );
			
		} else if ( geom == VOCAB_CYL || geom == VOCAB_SCYL ) {
			
			double r = command.get(n).asDouble(); n++; // radius
			double h = command.get(n).asDouble(); n++; // height 
			double px = command.get(n).asDouble(); n++; // x position
			double py = command.get(n).asDouble(); n++; // y position  
			double pz = command.get(n).asDouble(); n++; // z position
			
			//if ( geom == VOCAB_CYL ) { world->newCylinder( r, h, QVector3D(px,py,pz) ); }
			//else if ( geom == VOCAB_SCYL ) { world->newSCylinder( r, h, QVector3D(px,py,pz) ); }
			
			primitive = new KinematicModel::Cylinder( r, h );
			composite->appendPrimitive( primitive );
			composite->setPosition( QVector3D(px,py,pz) );
			
			QString name;
            if ( geom == VOCAB_CYL ) { name = "cyl" + QString::number(++c); }
			else if ( geom == VOCAB_SCYL ) { name = "scyl" + QString::number(++sc); }
            composite->setName(name);
			reply.addString( name.toLocal8Bit().data() );
			
		} else if ( geom == VOCAB_BOX || geom == VOCAB_SBOX ) {
			
			double x = command.get(n).asDouble(); n++;  // x size
			double y = command.get(n).asDouble(); n++;  // y size  
			double z = command.get(n).asDouble(); n++;  // z size
			double px = command.get(n).asDouble(); n++; // x position
			double py = command.get(n).asDouble(); n++; // y position  
			double pz = command.get(n).asDouble(); n++; // z position

			//if ( geom == VOCAB_BOX ) { world->newBox( QVector3D(x,y,z), QVector3D(px,py,pz) ); }
			//else if ( geom == VOCAB_SBOX ) { world->newSBox( QVector3D(x,y,z), QVector3D(px,py,pz) ); }
			
			primitive = new KinematicModel::Box( QVector3D(x,y,z) );
			composite->appendPrimitive( primitive );
			composite->setPosition( QVector3D(px,py,pz) );
            
            QString name;
            if ( geom == VOCAB_BOX ) { name = "box" + QString::number(++b); }
			else if ( geom == VOCAB_SBOX ) { name = "sbox" + QString::number(++sb); }
            composite->setName(name);
			reply.addString( name.toLocal8Bit().data() );
		}
		else { reply.addString("MK ERROR: Unknown geometry... use '(s)sph', '(s)cyl', or '(s)box'"); }
		
		model->appendObject( composite );
	}
	catch (std::exception& e)
	{
		reply.clear();
		reply.addString(e.what());
	}
}