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