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.");
	}
}
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());
	}
}