Пример #1
0
void CPTG_RobotShape_Polygonal::add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines& gl_shape,
	const mrpt::poses::CPose2D& origin) const
{
	const int N = m_robotShape.size();
	if (N >= 2)
	{
		// Transform coordinates:
		mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
		for (int i = 0; i < N; i++)
		{
			origin.composePoint(
				m_robotShape[i].x, m_robotShape[i].y, 0, shap_x[i], shap_y[i],
				shap_z[i]);
		}

		gl_shape.appendLine(
			shap_x[0], shap_y[0], shap_z[0], shap_x[1], shap_y[1], shap_z[1]);
		for (int i = 0; i <= shap_x.size(); i++)
		{
			const int idx = i % shap_x.size();
			gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
		}
	}
}
Пример #2
0
/** Last time-step velocity (of the ref. point, in local coords) */
vec3 Block::getVelocityLocal() const
{
	vec3 local_vel;
	local_vel.vals[2] = m_dq.vals[2]; // omega remains the same.

	const mrpt::poses::CPose2D p(0,0, -m_q.yaw); // "-" means inverse pose
	p.composePoint(
		m_dq.vals[0],m_dq.vals[1],
		local_vel.vals[0],local_vel.vals[1]);
	return local_vel;
}
void CPTG_RobotShape_Circular::add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines &gl_shape,
	const mrpt::poses::CPose2D &origin) const 
{
	const double R = m_robotRadius;
	const int N = 21;
	// Transform coordinates:
	mrpt::math::CVectorDouble shap_x(N), shap_y(N),shap_z(N);
	for (int i=0;i<N;i++) {
		origin.composePoint(
			R*cos(i*2*M_PI/(N-1)),R*sin(i*2*M_PI/(N-1)), 0,
			shap_x[i],  shap_y[i],  shap_z[i]);
	}
	// Draw a "radius" to identify the "forward" orientation (phi=0)
	gl_shape.appendLine( origin.x(), origin.y(), .0, shap_x[0],shap_y[0],shap_z[0] );
	for (int i=1;i<=shap_x.size();i++) {
		const int idx = i % shap_x.size();
		gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
	}
}
Пример #4
0
// Aux function
void add_robotShape_to_setOfLines(
	const CVectorFloat &shap_x_,
	const CVectorFloat &shap_y_, 
	mrpt::opengl::CSetOfLines &gl_shape, 
	const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D () )
{
	const int N = shap_x_.size();
	if (N>=2 && N==shap_y_.size() )
	{
		// Transform coordinates:
		CVectorDouble shap_x(N), shap_y(N),shap_z(N);
		for (int i=0;i<N;i++) {
			origin.composePoint(
				shap_x_[i], shap_y_[i], 0,
				shap_x[i],  shap_y[i],  shap_z[i]);
		}

		gl_shape.appendLine( shap_x[0], shap_y[0], shap_z[0], shap_x[1],shap_y[1],shap_z[1] );
		for (int i=0;i<=shap_x.size();i++) {
			const int idx = i % shap_x.size();
			gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
		}
	}
}
Пример #5
0
// See docs in base class.
void DefaultFriction::evaluate_friction(const FrictionBase::TFrictionInput &input, mrpt::math::TPoint2D &out_result_force_local) const
{
	// Rotate wheel velocity vector from veh. frame => wheel frame
	const mrpt::poses::CPose2D wRot(0,0,input.wheel.yaw);
	const mrpt::poses::CPose2D wRotInv(0,0,-input.wheel.yaw);
	mrpt::math::TPoint2D vel_w; 
	wRotInv.composePoint(input.wheel_speed,vel_w);

	// Action/Reaction, slippage, etc:
	// --------------------------------------
	const double mu = m_mu;
	const double gravity = m_my_vehicle.getWorldObject()->get_gravity();
	const double partial_mass = input.weight/gravity  + input.wheel.mass;
	const double max_friction = mu * partial_mass * gravity;

	// 1) Lateral friction (decoupled sub-problem)
	// --------------------------------------------
	double wheel_lat_friction=0.0;  // direction: +y local wrt the wheel
	{
		// Impulse required to step the lateral slippage:
		wheel_lat_friction = -vel_w.y * partial_mass/ input.context.dt;

		wheel_lat_friction = b2Clamp(wheel_lat_friction, -max_friction,max_friction);
	}

	// 2) Longitudinal friction (decoupled sub-problem)
	// -------------------------------------------------
	double wheel_long_friction=0.0; // direction: +x local wrt the wheel
	
	// (eq. 1)==> desired impulse in wheel spinning speed.
	// wheel_C_lon_vel = vel_w.x - input.wheel.w * 0.5*input.wheel.diameter

	// It should be = 0 for no slippage (nonholonomic constraint): find out required wheel \omega:case '4':
	const double R = 0.5*input.wheel.diameter; // Wheel radius
	const double lon_constraint_desired_wheel_w = vel_w.x / R;
	const double desired_wheel_w_impulse = (lon_constraint_desired_wheel_w-input.wheel.getW());
	const double desired_wheel_alpha = desired_wheel_w_impulse / input.context.dt;
	
	// (eq. 3)==> Find out F_r
	// Iyy_w * \Delta\omega_w = dt*\tau-  R*dt*Fri    -C_damp * \omega_w * dt
	// "Damping" / internal friction of the wheel's shaft, etc.
	const double C_damping = m_C_damping;
	//const mrpt::math::TPoint2D wheel_damping(- C_damping * input.wheel_speed.x, 0.0);

	const double I_yy = input.wheel.Iyy;
	double F_friction_lon = ( input.motor_torque - I_yy*desired_wheel_alpha - C_damping*input.wheel.getW() )/R;

	// Slippage: The friction with the ground is not infinite:
	F_friction_lon = b2Clamp(F_friction_lon, -max_friction,max_friction);
	
	// Recalc wheel ang. velocity impulse with this reduced force:
	const double actual_wheel_alpha = ( input.motor_torque - R * F_friction_lon - C_damping*input.wheel.getW() )/I_yy;
	
	// Apply impulse to wheel's spinning:
	input.wheel.setW( input.wheel.getW() + actual_wheel_alpha * input.context.dt );

	wheel_long_friction = F_friction_lon;

	
	// Resultant force: In local (x,y) coordinates (Newtons) wrt the Wheel
	// -----------------------------------------------------------------------
	const mrpt::math::TPoint2D result_force_wrt_wheel(wheel_long_friction ,wheel_lat_friction);

	// Rotate to put: Wheel frame ==> vehicle local framework:
	wRot.composePoint(result_force_wrt_wheel, out_result_force_local);
}
Пример #6
0
/** Class factory: Creates a vehicle from XML description of type "<vehicle>...</vehicle>".  */
Block* Block::factory(World* parent, const rapidxml::xml_node<char> *root)
{
	using namespace std;
	using namespace rapidxml;

	if (!root) throw runtime_error("[Block::factory] XML node is NULL");
	if (0!=strcmp(root->name(),"block")) throw runtime_error(mrpt::format("[Block::factory] XML root element is '%s' ('block' expected)",root->name()));

	// "class": When there is a 'class="XXX"' attribute, look for each parameter
	//  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();
			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);
		}
	}

	// Build object (we don't use class factory for blocks)
	// ----------------------------------------------------
	Block *block = new Block(parent);

	// Init params
	// -------------------------------------------------
	// attrib: name
	{
		const xml_attribute<> *attrib_name = root->first_attribute("name");
		if (attrib_name && attrib_name->value())
		{
			block->m_name = attrib_name->value();
		}
		else
		{
			// Default name:
			static int cnt =0;
			block->m_name = mrpt::format("block%i",++cnt);
		}
	}

	// (Mandatory) initial pose:
	{
		const xml_node<> *node = block_root_node.first_node("init_pose");
		if (!node) throw runtime_error("[Block::factory] Missing XML node <init_pose>");

		if (3!= ::sscanf(node->value(),"%lf %lf %lf",&block->m_q.x,&block->m_q.y,&block->m_q.yaw))
			throw runtime_error("[Block::factory] Error parsing <init_pose>...</init_pose>");
		block->m_q.yaw *= M_PI/180.0; // deg->rad
	}

	// (Optional) initial vel:
	{
		const xml_node<> *node = block_root_node.first_node("init_vel");
		if (node)
		{
			if (3!= ::sscanf(node->value(),"%lf %lf %lf",&block->m_dq.vals[0],&block->m_dq.vals[1],&block->m_dq.vals[2]))
				throw runtime_error("[Block::factory] Error parsing <init_vel>...</init_vel>");
			block->m_dq.vals[2] *= M_PI/180.0; // deg->rad

			// Convert twist (velocity) from local -> global coords:
			const mrpt::poses::CPose2D pose(0,0,block->m_q.yaw); // Only the rotation
			pose.composePoint(
				block->m_dq.vals[0], block->m_dq.vals[1],
				block->m_dq.vals[0], block->m_dq.vals[1] );
		}
	}

	// 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["restitution"]   = TParamEntry("%lf", &block->m_restitution);
	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();
	block->create_multibody_system(b2world);

	if (block->m_b2d_block_body)
	{
		// Init pos:
		block->m_b2d_block_body->SetTransform( b2Vec2( block->m_q.x, block->m_q.y ),  block->m_q.yaw );
		// Init vel:
		block->m_b2d_block_body->SetLinearVelocity( b2Vec2(block->m_dq.vals[0], block->m_dq.vals[1] ) );
		block->m_b2d_block_body->SetAngularVelocity(block->m_dq.vals[2] );
	}

	return block;
}