diff --git a/cfg/mvsimNode.cfg b/cfg/mvsimNode.cfg index 3df36e8e..e3bac4c6 100755 --- a/cfg/mvsimNode.cfg +++ b/cfg/mvsimNode.cfg @@ -12,7 +12,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("show_gui", bool_t, 0, "Show MVSim 3D GUI", True) -gen.add("simul_timestep", double_t, 0, "Simulation timestep (in seconds)", 0.005, 0.001, 0.050) exit(gen.generate(PACKAGE, "mvsim_node", "mvsimNode")) diff --git a/libmvsim/include/mvsim/WorldElements/OccupancyGridMap.h b/libmvsim/include/mvsim/WorldElements/OccupancyGridMap.h index a294dd00..05907a30 100644 --- a/libmvsim/include/mvsim/WorldElements/OccupancyGridMap.h +++ b/libmvsim/include/mvsim/WorldElements/OccupancyGridMap.h @@ -47,17 +47,18 @@ namespace mvsim TFixturePtr() : fixture(NULL) {} }; - struct TInfoPerVeh + struct TInfoPerCollidableobj { + float max_obstacles_ranges; mrpt::poses::CPose2D pose; mrpt::slam::CObservation2DRangeScanPtr scan; b2Body* collide_body; std::vector collide_fixtures; - TInfoPerVeh() : collide_body(NULL) {} + TInfoPerCollidableobj() : max_obstacles_ranges(0), collide_body(NULL) {} }; - std::vector m_obstacles_for_each_veh; + std::vector m_obstacles_for_each_obj; std::vector m_gl_obs_clouds; mrpt::synch::CCriticalSection m_gl_obs_clouds_buffer_cs; diff --git a/libmvsim/src/Block.cpp b/libmvsim/src/Block.cpp index 8e83a575..0e46581a 100644 --- a/libmvsim/src/Block.cpp +++ b/libmvsim/src/Block.cpp @@ -37,8 +37,8 @@ Block::Block(World *parent) : m_q(0,0,0,0,0,0), m_dq(0,0,0), m_mass(30.0), - m_block_z_min(0.05), - m_block_z_max(0.6), + m_block_z_min(0.0), + m_block_z_max(1.0), m_block_color(0x00,0x00,0xff), m_block_com(.0,.0), m_lateral_friction(0.5), @@ -105,13 +105,14 @@ Block* Block::factory(World* parent, const rapidxml::xml_node *root) // in the set of "root" + "class_root" XML nodes: // -------------------------------------------------------------------------------- JointXMLnode<> block_root_node; + const rapidxml::xml_node* class_root=NULL; { block_root_node.add(root); // Always search in root. Also in the class root, if any: const xml_attribute<> *block_class = root->first_attribute("class"); if (block_class) { const string sClassName = block_class->value(); - const rapidxml::xml_node* class_root= block_classes_registry.get(sClassName); + class_root= block_classes_registry.get(sClassName); if (!class_root) throw runtime_error(mrpt::format("[Block::factory] Block class '%s' undefined",sClassName.c_str() )); block_root_node.add(class_root); @@ -166,6 +167,27 @@ Block* Block::factory(World* parent, const rapidxml::xml_node *root) } } + // Params: + std::map params; + params["mass"] = TParamEntry("%lf", &block->m_mass); + params["zmin"] = TParamEntry("%lf", &block->m_block_z_min ); + params["zmax"] = TParamEntry("%lf", &block->m_block_z_max ); + params["ground_friction"] = TParamEntry("%lf", &block->m_ground_friction); + params["lateral_friction"] = TParamEntry("%lf", &block->m_lateral_friction); + params["color"] = TParamEntry("%color", &block->m_block_color ); + + parse_xmlnode_children_as_param(*root, params,"[Block::factory]" ); + if (class_root) + parse_xmlnode_children_as_param(*class_root, params,"[Block::factory]" ); + + // Shape node (optional, fallback to default shape if none found) + const rapidxml::xml_node * xml_shape = block_root_node.first_node("shape"); + if (xml_shape) + { + mvsim::parse_xmlnode_shape(*xml_shape, block->m_block_poly, "[Block::factory]"); + block->updateMaxRadiusFromPoly(); + } + // Register bodies, fixtures, etc. in Box2D simulator: // ---------------------------------------------------- b2World* b2world = parent->getBox2DWorld(); @@ -206,45 +228,59 @@ void Block::simul_pre_timestep(const TSimulContext &context) // Apply motor forces/torques: //this->invoke_motor_controllers(context,m_torque_per_wheel); - // Apply friction model: - const double weight = m_mass * getWorldObject()->get_gravity(); std::vector force_vectors; // For visualization only -#if 0 - std::vector wheels_vels; - getWheelsVelocityLocal(wheels_vels,getVelocityLocal()); - for (size_t i=0;iget_gravity() / nContactPoints; + const double mu = m_ground_friction; + const double max_friction = mu * weight_per_contact_point; + + + // Location (local coords) of each contact-point: + const vec2 pt_loc[nContactPoints] = { + vec2( m_max_radius,0), + vec2(-m_max_radius,0) + }; + const double block_vx = m_dq.vals[0]; + const double block_vy = m_dq.vals[1]; + const double w = m_dq.vals[2]; // block \omega + + // Each point velocity is: + // v_point = v + \omega \times wheel_pos + // => + // v_point = v + ( -w*y, w*x ) + for (size_t i=0;i Forwards is negative - fi.weight = weightPerWheel; - fi.wheel_speed = wheels_vels[i]; + // X friction + double x_friction = -vx * m_mass/ context.dt; // Impulse required to step the slippage: + x_friction = b2Clamp(x_friction, -max_friction,max_friction); - // eval friction: - mrpt::math::TPoint2D net_force_; - m_friction->evaluate_friction(fi,net_force_); + // Y friction + double y_friction = -vy * m_mass/ context.dt; // Impulse required to step the slippage: + y_friction = b2Clamp(y_friction, -max_friction,max_friction); // Apply force: - const b2Vec2 wForce = m_b2d_block_body->GetWorldVector(b2Vec2(net_force_.x,net_force_.y)); // Force vector -> world coords - const b2Vec2 wPt = m_b2d_block_body->GetWorldPoint( b2Vec2( w.x, w.y) ); // Application point -> world coords - //printf("w%i: Lx=%6.3f Ly=%6.3f | Gx=%11.9f Gy=%11.9f\n",(int)i,net_force_.x,net_force_.y,wForce.x,wForce.y); - + const b2Vec2 wForce(x_friction,y_friction); + const b2Vec2 wPt = m_b2d_block_body->GetWorldPoint( b2Vec2(pt_loc[i].vals[0], pt_loc[i].vals[1]) ); // Application point -> world coords m_b2d_block_body->ApplyForce( wForce,wPt, true/*wake up*/); // save it for optional rendering: if (m_world->m_gui_options.show_forces) { const double forceScale = m_world->m_gui_options.force_scale; // [meters/N] - const mrpt::math::TPoint3D pt1(wPt.x,wPt.y, m_chassis_z_max*1.1 + m_q.z ); + const mrpt::math::TPoint3D pt1(wPt.x,wPt.y, m_block_z_max*1.1 + m_q.z ); const mrpt::math::TPoint3D pt2 = pt1 + mrpt::math::TPoint3D(wForce.x,wForce.y, 0)*forceScale; force_vectors.push_back( mrpt::math::TSegment3D( pt1,pt2 )); } - } -#endif + + } // for each contact point + // Save forces for optional rendering: if (m_world->m_gui_options.show_forces) diff --git a/libmvsim/src/World.cpp b/libmvsim/src/World.cpp index d9d5a071..8d3f916a 100644 --- a/libmvsim/src/World.cpp +++ b/libmvsim/src/World.cpp @@ -108,26 +108,20 @@ void World::internal_one_timestep(double dt) // 1) Pre-step { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.prestep.veh"); + mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.prestep"); for(TListVehicles::iterator it=m_vehicles.begin();it!=m_vehicles.end();++it) it->second->simul_pre_timestep(context); - } - { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.prestep.sensors"); + for(TListVehicles::iterator it=m_vehicles.begin();it!=m_vehicles.end();++it) { VehicleBase::TListSensors &sensors = it->second->getSensors(); for (VehicleBase::TListSensors::iterator itSen=sensors.begin();itSen!=sensors.end();++itSen) { (*itSen)->simul_pre_timestep(context); } } - } - { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.prestep.blocks"); + for(TListBlocks::iterator it=m_blocks.begin();it!=m_blocks.end();++it) it->second->simul_pre_timestep(context); - } - { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.prestep.world"); + for(std::list::iterator it=m_world_elements.begin();it!=m_world_elements.end();++it) (*it)->simul_pre_timestep(context); } @@ -161,26 +155,20 @@ void World::internal_one_timestep(double dt) // 4) Post-step: { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.poststep.veh"); + mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.4.poststep"); for(TListVehicles::iterator it=m_vehicles.begin();it!=m_vehicles.end();++it) it->second->simul_post_timestep(context); - } - { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.poststep.sensors"); + for(TListVehicles::iterator it=m_vehicles.begin();it!=m_vehicles.end();++it) { VehicleBase::TListSensors &sensors = it->second->getSensors(); for (VehicleBase::TListSensors::iterator itSen=sensors.begin();itSen!=sensors.end();++itSen) { (*itSen)->simul_post_timestep(context); } } - } - { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.poststep.blocks"); + for(TListBlocks::iterator it=m_blocks.begin();it!=m_blocks.end();++it) it->second->simul_post_timestep(context); - } - { - mrpt::utils::CTimeLoggerEntry tle(m_timlogger,"timestep.0.poststep.world"); + for(std::list::iterator it=m_world_elements.begin();it!=m_world_elements.end();++it) (*it)->simul_post_timestep(context); } diff --git a/libmvsim/src/WorldElements/OccupancyGridMap.cpp b/libmvsim/src/WorldElements/OccupancyGridMap.cpp index 00a617ea..7c0c44b2 100644 --- a/libmvsim/src/WorldElements/OccupancyGridMap.cpp +++ b/libmvsim/src/WorldElements/OccupancyGridMap.cpp @@ -90,9 +90,9 @@ void OccupancyGridMap::gui_update( mrpt::opengl::COpenGLScene &scene) m_gl_grid = mrpt::opengl::CSetOfObjects::Create(); SCENE_INSERT_Z_ORDER(scene,0, m_gl_grid); } - if (m_gl_obs_clouds.size()!=m_obstacles_for_each_veh.size()) + if (m_gl_obs_clouds.size()!=m_obstacles_for_each_obj.size()) { - m_gl_obs_clouds.resize( m_obstacles_for_each_veh.size() ); + m_gl_obs_clouds.resize( m_obstacles_for_each_obj.size() ); } // 1st call OR gridmap changed? @@ -124,40 +124,59 @@ void OccupancyGridMap::gui_update( mrpt::opengl::COpenGLScene &scene) m_gl_obs_clouds_buffer.clear(); } // end lock - } + void OccupancyGridMap::simul_pre_timestep(const TSimulContext &context) { - // For each vehicle, create a ground body with fixtures at each scanned point around the vehicle, so it can collide with the environment: - const World::TListVehicles & lstVehs = this->m_world->getListOfVehicles(); + // Make a list of objects subject to collide with the occupancy grid: + // - Vehicles + // - Blocks + { + const World::TListVehicles & lstVehs = this->m_world->getListOfVehicles(); + const World::TListBlocks & lstBlocks = this->m_world->getListOfBlocks(); + const size_t nObjs = lstVehs.size() + lstBlocks.size(); - // Upon first usage, reserve mem: - m_obstacles_for_each_veh.resize( lstVehs.size() ); + m_obstacles_for_each_obj.resize( nObjs ); + size_t obj_idx=0; + for (World::TListVehicles::const_iterator itVeh=lstVehs.begin();itVeh!=lstVehs.end();++itVeh,++obj_idx) + { + TInfoPerCollidableobj &ipv = m_obstacles_for_each_obj[obj_idx]; + ipv.max_obstacles_ranges = itVeh->second->getMaxVehicleRadius() * 1.50f; + const mrpt::math::TPose3D &pose = itVeh->second->getPose(); + ipv.pose = mrpt::poses::CPose2D(pose.x,pose.y, 0 /* angle=0, no need to rotate everything to later rotate back again! */); + } + for (World::TListBlocks::const_iterator it=lstBlocks.begin();it!=lstBlocks.end();++it,++obj_idx) + { + TInfoPerCollidableobj &ipv = m_obstacles_for_each_obj[obj_idx]; + ipv.max_obstacles_ranges = it->second->getMaxBlockRadius() * 1.50f; + const mrpt::math::TPose3D &pose = it->second->getPose(); + ipv.pose = mrpt::poses::CPose2D(pose.x,pose.y, 0 /* angle=0, no need to rotate everything to later rotate back again! */); + } + } + // For each object, create a ground body with fixtures at each scanned point around the vehicle, so it can collide with the environment: + // Upon first usage, reserve mem: { // lock mrpt::synch::CCriticalSectionLocker csl( &m_gl_obs_clouds_buffer_cs ); - m_gl_obs_clouds_buffer.resize(lstVehs.size()); + const size_t nObjs = m_obstacles_for_each_obj.size(); + m_gl_obs_clouds_buffer.resize(nObjs); - size_t veh_idx=0; - for (World::TListVehicles::const_iterator itVeh=lstVehs.begin();itVeh!=lstVehs.end();++itVeh, ++veh_idx) + for (size_t obj_idx=0;obj_idxsecond->getMaxVehicleRadius() * 1.50f; + const float veh_max_obstacles_ranges = ipv.max_obstacles_ranges; const float occup_threshold = 0.5f; const size_t nRays = 50; - const mrpt::math::TPose3D &pose = itVeh->second->getPose(); scan->aperture = 2.0*M_PI; // 360 field of view scan->maxRange = veh_max_obstacles_ranges; - ipv.pose = mrpt::poses::CPose2D(pose.x,pose.y, 0 /* angle=0, no need to rotate everything to latter rotate back again! */); - m_grid.laserScanSimulator( *scan, ipv.pose, @@ -183,7 +202,7 @@ void OccupancyGridMap::simul_pre_timestep(const TSimulContext &context) // GL: // 1st usage? - mrpt::opengl::CPointCloudPtr & gl_pts = m_gl_obs_clouds_buffer[veh_idx]; + mrpt::opengl::CPointCloudPtr & gl_pts = m_gl_obs_clouds_buffer[obj_idx]; if (m_show_grid_collision_points) { gl_pts = mrpt::opengl::CPointCloud::Create(); @@ -247,7 +266,7 @@ void OccupancyGridMap::simul_pre_timestep(const TSimulContext &context) } } } - } // end for veh_idx + } // end for obj_idx } // end lock diff --git a/libmvsim/src/xml_utils.cpp b/libmvsim/src/xml_utils.cpp index 8e2b6334..66fb2dc4 100644 --- a/libmvsim/src/xml_utils.cpp +++ b/libmvsim/src/xml_utils.cpp @@ -133,6 +133,7 @@ bool mvsim::parse_xmlnode_as_param( return false; } + /** Call \a parse_xmlnode_as_param() for all children nodes of the given node. */ void mvsim::parse_xmlnode_children_as_param( const rapidxml::xml_node &root, diff --git a/libmvsim/src/xml_utils.h b/libmvsim/src/xml_utils.h index 535471ba..fd30a771 100644 --- a/libmvsim/src/xml_utils.h +++ b/libmvsim/src/xml_utils.h @@ -63,8 +63,19 @@ namespace mvsim const std::map ¶ms, const char* function_name_context=""); + template + void parse_xmlnodelist_children_as_param( + NODE_LIST &lst_nodes, + const std::map ¶ms, + const char* function_name_context="") + { + for (typename NODE_LIST::iterator it= lst_nodes.begin();it!=lst_nodes.end();++it) + parse_xmlnode_children_as_param(**it, params,function_name_context); + } + + // Bits: - + /** Parses a string like "XXX YYY PHI" with X,Y in meters, PHI in degrees, and returns * a vec3 with [x,y,phi] with angle in radians. Raises an exception upon malformed string. */ diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index a5bb609b..e0fc571a 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -110,8 +110,6 @@ void MVSimNode::configCallback(mvsim::mvsimNodeConfig &config, uint32_t level) // message = config.message.c_str(); ROS_INFO("MVSimNode::configCallback() called."); - mvsim_world_.set_simul_timestep( config.simul_timestep ); - if (mvsim_world_.is_GUI_open() && !config.show_gui) mvsim_world_.close_GUI(); diff --git a/mvsim_tutorial/mvsim_demo_2robots.rviz b/mvsim_tutorial/mvsim_demo_2robots.rviz index 2b3ee19e..a57dbc00 100644 --- a/mvsim_tutorial/mvsim_demo_2robots.rviz +++ b/mvsim_tutorial/mvsim_demo_2robots.rviz @@ -6,9 +6,7 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /TF1 - - /TF1/Frames1 - Splitter Ratio: 0.5 + Splitter Ratio: 0.557312 Tree Height: 362 - Class: rviz/Selection Name: Selection @@ -215,7 +213,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 29.513 + Distance: 24.1546 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 @@ -227,10 +225,10 @@ Visualization Manager: Z: -0.341872 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.789797 + Pitch: 0.874797 Target Frame: Value: Orbit (rviz) - Yaw: 1.6104 + Yaw: 1.5154 Saved: ~ Window Geometry: Displays: diff --git a/mvsim_tutorial/mvsim_demo_2robots.xml b/mvsim_tutorial/mvsim_demo_2robots.xml index 67389dbb..aa1e0a0d 100644 --- a/mvsim_tutorial/mvsim_demo_2robots.xml +++ b/mvsim_tutorial/mvsim_demo_2robots.xml @@ -1,6 +1,6 @@ - 0.005 + 0.001 3 3 @@ -134,6 +134,16 @@ Obstacle blocks classes definition ====================================== --> + 1 + #0080ff + 2 + 0.5 + + -1.0 -0.6 + -1.0 0.6 + 1.0 0.5 + 1.0 -0.5 +