Beispiel #1
0
	Primitives OutdoorEnv::getAllPrimitives()	{
		Primitives result;
		if (r_sky->getBool()) {
#if 1
			result.push_back(m_skybox12);
			result.push_back(m_skybox34);
			result.push_back(m_skybox5);
#else
			result.push_back(m_skydome);
#endif
		}

		if (r_water->getBool()) {
			result.push_back(m_oceanMesh);
		}
		return result;
	}
Beispiel #2
0
    // generate 3 goal boxes return Primitives(vector of Primitive*) of boxes to add to grippables
    void generate_boxes(GlobalData& global)
    {
        double length = 1.38;
        double width = 1.38;
        double height = 0.9;
        Substance material(5.0,10.0,99.0,1.0);
        double mass = 0.1;
        PassiveBox* b1;
        b1 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height),mass);
        b1->setColor(Color(0,1,0));
        b1->setSubstance(material);
        b1->setPose(osg::Matrix::rotate(0, 0,0, 1) * osg::Matrix::translate(-5,0,1.5));
        global.obstacles.push_back(b1);

        PassiveBox* b2;
        b2 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height),mass);
        b2->setColor(Color(1,0,0));
        b2->setSubstance(material);
        b2->setPose(osg::Matrix::rotate(0, 0,0, 1) * osg::Matrix::translate(5,0,1.5));
        global.obstacles.push_back(b2);

        PassiveBox* b3;
        b3 = new PassiveBox(odeHandle, osgHandle, osg::Vec3(length, width, height),mass);
        b3->setColor(Color(1,0,0));
        b3->setSubstance(material);
        b3->setPose(osg::Matrix::rotate(0, 0,0, 1) * osg::Matrix::translate(0,-5,1.5));
        global.obstacles.push_back(b3);

        boxPrimitives.push_back(b1->getMainPrimitive());
        boxPrimitives.push_back(b2->getMainPrimitive());
        boxPrimitives.push_back(b3->getMainPrimitive());

        // adding boxes to obstacle vector so it can be connected to sensors
        relative_sensor_obst.push_back(b1);
        relative_sensor_obst.push_back(b2);
        relative_sensor_obst.push_back(b3);

    }
Beispiel #3
0
    // starting function (executed once at the beginning of the simulation loop)
    virtual bool restart(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
    {

        std::cout << "\n begin restart " << currentCycle << "\n";

        std::cout<<"Current Cycle"<<this->currentCycle<<std::endl;

        // remove agents
        while (global.agents.size() > 0)
        {
            OdeAgent* agent = *global.agents.begin();

            AbstractController* controller = agent->getController();

            OdeRobot* robot = agent->getRobot();
            AbstractWiring* wiring = agent->getWiring();

            global.configs.erase(std::find(global.configs.begin(),
                                           global.configs.end(), controller));

            delete robot;
            delete wiring;

            global.agents.erase(global.agents.begin());

        }
        // clean the playgrounds
        while (global.obstacles.size() > 0)
        {
            std::vector<AbstractObstacle*>::iterator iter =
                global.obstacles.begin();
            delete (*iter);
            global.obstacles.erase(iter);
        }
        boxPrimitives.clear();
        relative_sensor_obst.clear();
        grippables.clear();

        ///////////////Recreate Robot Start//////////////////////////////////////////////////////////////////////////////////////
        /**************************************************************************************************
        ***			Set up Environment
        **************************************************************************************************/
        setup_Playground(global);

        /**************************************************************************************************
        ***			Set up 4 landmark and 1 goal spheres
        **************************************************************************************************/

        generate_spheres(global);

        /**************************************************************************************************
        ***			Set up 3 pushable boxes and add the first one as graspable
        ************************************************************************************************/

        generate_boxes(global);
        grippables.push_back(boxPrimitives[0]);
//		grippables.push_back(boxPrimitives[1]);
//		grippables.push_back(boxPrimitives[2]);

        /**************************************************************************************************
        ***			Set up robot and controller
        **************************************************************************************************/

        //1) Activate IR sensors
        FourWheeledConfGripper fconf = FourWheeledRPosGripper::getDefaultConf();

        ///2) relative sensors
        for (unsigned int i=0; i < relative_sensor_obst.size(); i++) {
            fconf.rpos_sensor_references.push_back(relative_sensor_obst.at(i)->getMainPrimitive());
        }
        vehicle = new FourWheeledRPosGripper(odeHandle, osgHandle, fconf);

        /****Initial position of Nimm4******/
        Pos pos(0.0 , 0.0 , 1.0);
        //setting position and orientation
        vehicle->place(osg::Matrix::rotate(0, 0, 0, 1) *osg::Matrix::translate(pos));

        // only set new grippables otherwise keep old controller
        qcontroller->setGrippablesAndVehicle(vehicle,grippables);
        global.configs.push_back(qcontroller);

        // create pointer to one2onewiring
        AbstractWiring*  wiring = new One2OneWiring(new ColorUniformNoise(0.1));

        // create pointer to agent
        OdeAgent* agent = new OdeAgent(global);

        agent->init(qcontroller, vehicle, wiring);///////////// Initial controller!!!
        global.agents.push_back(agent);


        std::cout << "\n end restart " << currentCycle << "\n";
        // restart!

        qcontroller->setReset(false);


        return true;

    }
Beispiel #4
0
    // starting function (executed once at the beginning of the simulation loop)
    void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global)
    {

        /**************************************************************************************************
        ***			Camera Position
        **************************************************************************************************/
        //setCameraHomePos(Pos(0, 40, 10),  Pos(0, 0, 0)); // viewing full scene from side
        setCameraHomePos(Pos(0, 5, 5),  Pos(0, 0, 0));
        //setCameraHomePos(Pos(0, 20, 20),  Pos(0, 0, 0));

        /**************************************************************************************************
        ***			Simulation Parameters
        **************************************************************************************************/
        //1) - set noise to 0.1
        global.odeConfig.noise= 0.0;//0.02;//0.05;
        //2) - set controlinterval -> default = 1
        global.odeConfig.setParam("controlinterval", 1);/*update frequency of the simulation ~> amos = 20*/
        //3) - set simulation setp size
        global.odeConfig.setParam("simstepsize", 0.01); /*stepsize of the physical simulation (in seconds)*/
        //Update frequency of simulation = 1*0.01 = 0.01 s ; 100 Hz
        //4) - set gravity if not set then it uses -9.81 =earth gravity
        //global.odeConfig.setParam("gravity", -9.81);

        /**************************************************************************************************
        ***			Set up Environment
        **************************************************************************************************/

        setup_Playground(global);
        Substance material(5.0,10.0,99.0,1.0);
        this->setGroundSubstance(material);
        /**************************************************************************************************
        ***			Set up 4 landmark and 1 goal spheres
        **************************************************************************************************/

        // not being used atm
        //generate_spheres(global);

        /**************************************************************************************************
        ***			Set up 3 pushable boxes and add the first one as graspable
        ************************************************************************************************/

        generate_boxes(global);
        grippables.push_back(boxPrimitives[0]);
//		grippables.push_back(boxPrimitives[1]);
//		grippables.push_back(boxPrimitives[2]);

        /**************************************************************************************************
        ***			Set up robot and controller
        **************************************************************************************************/

        //1) Activate IR sensors
        FourWheeledConfGripper fconf = FourWheeledRPosGripper::getDefaultConf();

        ///2) relative sensors
        for (unsigned int i=0; i < relative_sensor_obst.size(); i++) {
            fconf.rpos_sensor_references.push_back(relative_sensor_obst.at(i)->getMainPrimitive());
        }
        vehicle = new FourWheeledRPosGripper(odeHandle, osgHandle, fconf);

        /****Initial position of Nimm4******/
        Pos pos(0.0 , 0.0 , 1.0);
        //setting position and orientation
        vehicle->place(osg::Matrix::rotate(0, 0, 0, 1) *osg::Matrix::translate(pos));

        qcontroller = new ASLController("1","1");
//		qcontroller = new FSMController("1","1", vehicle, grippables);
        qcontroller->setGrippablesAndVehicle(vehicle,grippables);
        global.configs.push_back(qcontroller);

        // create pointer to one2onewiring
        AbstractWiring*  wiring = new One2OneWiring(new ColorUniformNoise(0.1));

        // create pointer to agent
        OdeAgent* agent = new OdeAgent(global);

        agent->init(qcontroller, vehicle, wiring);///////////// Initial controller!!!
        global.agents.push_back(agent);


    }
Beispiel #5
0
bool
Geometry::flatten()
{
    Logger log = getLogger( package + "." + m_id + ".flatten" );
    if( !hasSharedInputs() ) {
        SCENELOG_WARN( log, "No need to flatten a geometry without shared inputs." );
        return true;
    }

    struct Tuple {
        unsigned int    m_tuple[ VERTEX_SEMANTIC_N ];
        bool operator<(const Tuple& b) const {
            for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
                if( m_tuple[i] < b.m_tuple[i] ) {
                    return true;
                }
                else if( m_tuple[i] > b.m_tuple[i] ) {
                    return false;
                }
            }
            return false;
        }

    };


//    VertexInput inputs[ VERTEX_SEMANTIC_N ] = m_vertex_inputs;
    VertexInput inputs[ VERTEX_SEMANTIC_N ];
    for (int i = 0; i < VERTEX_SEMANTIC_N; ++i) {
        inputs[i] = m_vertex_inputs[i];
    }

    std::map< Tuple, unsigned int> remap;

    std::vector< int > indices;
    std::vector< unsigned int > offsets;

    for( auto it=m_primitive_sets.begin(); it!=m_primitive_sets.end(); ++it ) {

        SCENELOG_DEBUG( log, "Processing primitive " );

        unsigned int tuple_offsets[ VERTEX_SEMANTIC_N ];
        for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
            tuple_offsets[i] = ~0u;
        }

        // First, set tuple index for the common sources (defined in <vertex>).
        // If we have no shared inputs, this defaults to zero. Otherwise, we
        // grap the tuple offset from VERTEX_POSITION (which is an alias for
        // semantic=VERTEX within a shared input).
        unsigned int vertex_tuple_offset = 0;
        if( (*it)->hasSharedInputs() ) {
            if( (*it)->sharedInputEnabled(VERTEX_POSITION) ) {
                vertex_tuple_offset = (*it)->sharedInputTupleOffset( VERTEX_POSITION );
            }
            else {
                SCENELOG_WARN( log, "Input semantic VERTEX is required for shared inputs, giving up." );
                return false;
            }
        }
        for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
            if( unsharedInputEnabled( (VertexSemantic)i ) ) {
                tuple_offsets[i] = vertex_tuple_offset;
            }
        }

        // Then, process the shared inputs from the polygon set. We update the
        // input sources, and make sure that there are no inconsistencies, and
        // update the tuple indices.
        for( unsigned int i=1; i<VERTEX_SEMANTIC_N; i++ ) {
            VertexSemantic sem = (VertexSemantic)i;
            if( (*it)->sharedInputEnabled(sem) ) {
                if( inputs[i].m_enabled ) {
                    if( (inputs[i].m_source_buffer_id != (*it)->sharedInputSourceBuffer(sem) ) ||
                            (inputs[i].m_components       != (*it)->sharedInputComponents(sem)   ) ||
                            (inputs[i].m_count            != (*it)->sharedInputCount(sem)        ) ||
                            (inputs[i].m_stride           != (*it)->sharedInputStride(sem)       ) ||
                            (inputs[i].m_offset           != (*it)->sharedInputOffset(sem)       ) )
                    {
                        SCENELOG_ERROR( log, "Mismatch in shared input source definitions, giving up." );
                        return false;
                    }
                }
                else {
                    inputs[i].m_enabled          = true;
                    inputs[i].m_source_buffer_id = (*it)->sharedInputSourceBuffer(sem);
                    inputs[i].m_components       = (*it)->sharedInputComponents(sem);
                    inputs[i].m_count            = (*it)->sharedInputCount(sem);
                    inputs[i].m_stride           = (*it)->sharedInputStride(sem);
                    inputs[i].m_offset           = (*it)->sharedInputOffset(sem);
                }
                tuple_offsets[ i ] = (*it)->sharedInputTupleOffset( sem );
            }
        }
        for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
            if( tuple_offsets[i] != ~0u ) {
                SCENELOG_DEBUG( log, "sem=" << i << ", offset=" << tuple_offsets[i] );
            }
        }


        // Extract indicies
        offsets.push_back( indices.size() );
        if( (*it)->isIndexed() ) {
            SourceBuffer* index_buf = m_db.library<SourceBuffer>().get( (*it)->indexBufferId() );
            const int* ix = index_buf->intData();

            for( unsigned int p=0; p<(*it)->vertexCount(); p++ ) {

                Tuple t;
                for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
                    if( tuple_offsets[ i ] == ~0u ) {
                        t.m_tuple[i] = ~0u;
                    }
                    else {
                        t.m_tuple[i] = ix[ (*it)->indexOffset() +
                                           (*it)->sharedInputTupleSize()*p +
                                           tuple_offsets[ i ] ];
                    }
                }

                unsigned int ix;
                auto it = remap.find( t );
                if( it == remap.end() ) {
                    ix = remap.size();
                    remap[ t ] = ix;
                }
                else {
                    ix = it->second;
                }
                indices.push_back( ix );
            }
        }
        // Create indices for non-indexed shapes
        else {
            for( unsigned int p=0; p<(*it)->vertexCount(); p++ ) {
                Tuple t;
                for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
                    t.m_tuple[i] = ( tuple_offsets[i] == ~0u ? ~0u : p);
                }
                unsigned int ix;
                auto it = remap.find( t );
                if( it == remap.end() ) {
                    ix = remap.size();
                    remap[ t ] = ix;
                }
                else {
                    ix = it->second;
                }
                indices.push_back( ix );

            }
        }
    }
    offsets.push_back( indices.size() );



    // Create buffer
    unsigned int interleaved_offsets[ VERTEX_SEMANTIC_N +1 ];
    const float* interleaved_sources[ VERTEX_SEMANTIC_N ];
    interleaved_offsets[0] = 0;
    for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
        if( inputs[i].m_enabled ) {
            interleaved_offsets[i+1] = interleaved_offsets[i] + inputs[i].m_components;
            const SourceBuffer* sb = m_db.library<SourceBuffer>().get( inputs[i].m_source_buffer_id );
            if( sb == NULL ) {
                SCENELOG_ERROR( log, "Unable to resolve source buffer, giving up." );
                return false;
            }
            interleaved_sources[i] = sb->floatData();
        }
        else {
            interleaved_offsets[i+1] = interleaved_offsets[i];
            interleaved_sources[i] = NULL;
        }
        SCENELOG_DEBUG( log, "interleaved offset " << i << "=" << interleaved_offsets[i] );
    }
    unsigned int interleaved_stride = interleaved_offsets[ VERTEX_SEMANTIC_N ];
    if( interleaved_stride == 0 ) {
        SCENELOG_ERROR( log, "No source data, giving up" );
        return false;
    }

    std::vector<float> interleaved( interleaved_stride*remap.size() );
    for( auto it=remap.begin(); it!=remap.end(); ++it ) {
#if 0
        SCENELOG_TRACE( log, "map " <<
                        it->second << " = { " <<
                        it->first.m_tuple[0] << ", " <<
                        it->first.m_tuple[1] << ", " <<
                        it->first.m_tuple[2] << ", " <<
                        it->first.m_tuple[3] << ", " <<
                        it->first.m_tuple[4] << ", " <<
                        it->first.m_tuple[5] << ", " <<
                        it->first.m_tuple[6] << ", " <<
                        it->first.m_tuple[7] << "} " );
        float* dst = interleaved.data() + it->second*interleaved_stride;
#endif
        for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
            if( it->first.m_tuple[i] == ~0u ) {
                // ignore
            }
            else {
                std::copy_n( interleaved_sources[i] +
                             inputs[i].m_offset +
                             inputs[i].m_stride*it->first.m_tuple[i],
                             inputs[i].m_components,
                             interleaved.begin() +
                             it->second*interleaved_stride +
                             interleaved_offsets[i] );

            }

        }
        std::stringstream o;
        for( unsigned int i=0; i<interleaved_stride; i++ ) {
            if( i!=0 ) {
                o << ", ";
            }
            o << interleaved[ it->second*interleaved_stride + i ];
        }
        SCENELOG_TRACE( log, "data: " << o.str() );
    }

    SCENELOG_DEBUG( log, "Interleaved attribute tuple is of size " << interleaved_offsets[ VERTEX_SEMANTIC_N ] );

    SourceBuffer* interleaved_buffer = m_db.library<SourceBuffer>().add( m_id + "_attributes_float_interleaved" );
    if( interleaved_buffer == NULL ) {
        SCENELOG_ERROR( log, "Failed to create interleaved attribute buffer, giving up." );
        return false;
    }
    SourceBuffer* index_buffer = m_db.library<SourceBuffer>().add( m_id + "_flat_indices" );
    if( index_buffer == NULL ) {
        SCENELOG_ERROR( log, "Failed to create buffer for flattened indices, giving up" );
        return false;
    }

    interleaved_buffer->contents( interleaved );
    unsharedInputClearAll();
    for( unsigned int i=0; i<VERTEX_SEMANTIC_N; i++ ) {
        if( inputs[i].m_enabled ) {
            setVertexSource( (VertexSemantic)i,
                             interleaved_buffer->id(),
                             interleaved_offsets[i+1]-interleaved_offsets[i],
                             remap.size(),
                             interleaved_stride,
                             interleaved_offsets[i] );
        }
    }

    index_buffer->contents( indices );
    SCENELOG_DEBUG( log, "offsets.size=" << offsets.size() );
    SCENELOG_DEBUG( log, "m_primitive_sets.size=" << m_primitive_sets.size() );

    for( size_t i=0; i<m_primitive_sets.size(); i++ ) {
        Primitives* p = m_primitive_sets[i];
        p->clearSharedInputs();
        p->set( p->primitiveType(),
                (offsets[i+1]-offsets[i])/p->verticesPerPrimitive(),
                p->verticesPerPrimitive(),
                index_buffer->id(),
                offsets[i] );
    }

    return false;
}
Beispiel #6
0
	XMLElement* Mesh::GenerateMeshSegment(XMLDocument* doc, X3DObject& mesh, bool shouldExport, CString exportPath)
	{
		XMLElement* root = nullptr;

		if (MeshIsMassPrimitive(mesh))
		{
			Primitives* primitive = new Primitives();
			root = primitive->WritePrimitive(doc, mesh);
			delete primitive;
		}
		else
		{
			root = doc->NewElement("shape");
			root->SetAttribute(Constants::attrType, "ply");

			root->InsertEndChild(WriteElement(doc, Constants::attrString, Constants::attrFilename, (mesh.GetFullName() + L".ply").GetAsciiString()));

			XMLElement* trElement = doc->NewElement("transform");
			trElement->SetAttribute(Constants::attrName, "toWorld");
			root->InsertEndChild(trElement);

			MATH::CTransformation transform = mesh.GetKinematics().GetGlobal().GetTransform();
			trElement->InsertEndChild(WriteElementScale(doc, mesh));

			trElement->InsertEndChild(WriteSubElementRotation(doc, "x", transform.GetRotX()));
			trElement->InsertEndChild(WriteSubElementRotation(doc, "y", transform.GetRotY()));
			trElement->InsertEndChild(WriteSubElementRotation(doc, "z", transform.GetRotZ()));

			trElement->InsertEndChild(WriteElementTranslate(doc, mesh));

			Mitsuba::Material* material = new Mitsuba::Material();
			material->WriteMeshMaterial(doc, root, mesh);
			delete material;

			if (shouldExport)
			{
				bool isSolid = false;
				bool isSubdivide = false;
				LONG subLevel = 0;

				Property prop;
				mesh.GetPropertyFromName(L"MaSsObjectProperty", prop);
				if (prop.IsValid())
				{
					isSolid = prop.GetParameterValue(L"isSolid");
					isSubdivide = prop.GetParameterValue(L"isSubdivide");
					subLevel = prop.GetParameterValue(L"subLevel");
				}

				if (!isSubdivide)
				{
					Mitsuba::ExportPLY* exportPly = new Mitsuba::ExportPLY();
					exportPly->ExportPlyMeshBinary(mesh, exportPath, mesh.GetFullName() + L".ply", isSolid);
					delete exportPly;
				}
				else
				{
					/*#we should subdivide this mesh and then export
					#lm("We should subdivide the mesh")
					subOps = ap.ApplyOp("MeshSubdivideWithCenter", mesh)
					subMesh = ap.Selection(0)
					subOp = subOps(0)
					ap.SetValue(subOp.SubdivisionDepth, subLevel)
					ap.FreezeObj(subMesh, "", "")

					tM = mesh.Kinematics.Global.Transform.Matrix4
					oTrans = subMesh.Kinematics.Global.Transform
					tM.InvertInPlace()
					oTrans.SetMatrix4(tM)
					subMesh.Kinematics.Global.Transform = oTrans
					ap.ResetTransform(subMesh, "siCtr", "siSRT", "siXYZ")

					ExportPlyMesh02(subMesh, exportPath, mesh.FullName + ".ply")
					ap.DeleteObj(subMesh)*/
				}
			}
		}

		return root;
	}