progress with passive block dynamics

Former-commit-id: 360d25c

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"))

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<TFixturePtr> collide_fixtures;
 
-			TInfoPerVeh() : collide_body(NULL) {}
+			TInfoPerCollidableobj() : max_obstacles_ranges(0), collide_body(NULL) {}
 		};
 
-		std::vector<TInfoPerVeh>  m_obstacles_for_each_veh; 
+		std::vector<TInfoPerCollidableobj>  m_obstacles_for_each_obj; 
 		std::vector<mrpt::opengl::CSetOfObjectsPtr> m_gl_obs_clouds;
 
 		mrpt::synch::CCriticalSection m_gl_obs_clouds_buffer_cs;

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<char> *root)
 	//  in the set of "root" + "class_root" XML nodes:
 	// --------------------------------------------------------------------------------
 	JointXMLnode<> block_root_node;
+	const rapidxml::xml_node<char>* 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<char>* 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<char> *root)
 		}
 	}
 
+	// Params:
+	std::map<std::string,TParamEntry> 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<char> * 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<mrpt::math::TSegment3D> force_vectors; // For visualization only
-#if 0
-	std::vector<mrpt::math::TPoint2D> wheels_vels;
-	getWheelsVelocityLocal(wheels_vels,getVelocityLocal());
 
-	for (size_t i=0;i<nW;i++)
+	// Apply friction model:
+	// Use FOUR contact points between block & ground so there is a torque in the friction force
+	const size_t nContactPoints = 2;
+	const double weight_per_contact_point = m_mass * getWorldObject()->get_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<nContactPoints;i++)
 	{
-		// prepare data:
-		Wheel &w = getWheelInfo(i);
+		const double vx = block_vx - w * pt_loc[i].vals[1];
+		const double vy = block_vy + w * pt_loc[i].vals[0];
 
-		FrictionBase::TFrictionInput fi(context,w);
-		fi.motor_torque = -m_torque_per_wheel[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)

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<WorldElementBase*>::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<WorldElementBase*>::iterator it=m_world_elements.begin();it!=m_world_elements.end();++it)
 			(*it)->simul_post_timestep(context);
 	}

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_idx<nObjs;obj_idx++)
 		{
 			// 1) Simulate scan to get obstacles around the vehicle:
-			TInfoPerVeh &ipv = m_obstacles_for_each_veh[veh_idx];
+			TInfoPerCollidableobj &ipv = m_obstacles_for_each_obj[obj_idx];
 			mrpt::slam::CObservation2DRangeScanPtr &scan = ipv.scan;
 			// Upon first time, reserve mem:
 			if (!scan) scan = mrpt::slam::CObservation2DRangeScan::Create();
 
-			const float veh_max_obstacles_ranges = itVeh->second->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
 

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<char> &root,

libmvsim/src/xml_utils.h

@@ -63,8 +63,19 @@ namespace mvsim
 		const std::map<std::string,TParamEntry> &params,
 		const char* function_name_context="");
 
+	template <class NODE_LIST>
+	void parse_xmlnodelist_children_as_param(
+		NODE_LIST &lst_nodes,
+		const std::map<std::string,TParamEntry> &params,
+		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.
 	  */

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();