void SE_average<2>::append(const mrpt::poses::CPose2D& p, const double weight)
{
m_count += weight;
m_accum_x += weight * p.x();
m_accum_y += weight * p.y();
m_rot_part.append(p.phi(), weight);
}
void SE_average<2>::get_average(mrpt::poses::CPose2D &ret_mean) const
{
ASSERT_ABOVE_(m_count,0);
ret_mean.x( m_accum_x / m_count );
ret_mean.y( m_accum_y / m_count );
const_cast<SO_average<2>*>(&m_rot_part)->enable_exception_on_undeterminate = this->enable_exception_on_undeterminate;
ret_mean.phi ( m_rot_part.get_average() );
}
/** 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;
}
/*-------------------------------------------------------------
changeOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::changeOdometry(const mrpt::poses::CPose2D &newOdometry)
{
#if MRPT_HAS_ARIA
ArPose pos_actual;
THE_ROBOT->lock();
pos_actual.setPose( newOdometry.x()*1000, newOdometry.y()*1000, RAD2DEG( newOdometry.phi() ) );
THE_ROBOT->setDeadReconPose( pos_actual );
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
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]);
}
}
}
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]);
}
}
bool getCurrentPoseAndSpeeds( mrpt::poses::CPose2D &curPose, float &curV, float &curW)
{
robotSim.getRealPose( curPose );
double phi = M_PI/2 - curPose.phi();
if(phi<0)
{
cout << "changing" <<phi << endl;
phi+= 2*M_PI;
cout << "to" << phi << endl;
}
//curPose.phi(phi);
/*float temp = curPose.x();
curPose.x(curPose.y());
curPose.y(temp);*/
curV = robotSim.getV();
curW = robotSim.getW();
return true;
}
// 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]);
}
}
}
// 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);
}
/** 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;
}
