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