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