// Set any collision geometry on the hull, then Initialize() all subsystems
void TrackSoilBin::Initialize(const ChCoordsys<>& gear_Csys)
{
// initialize the drive gear, idler and track chain
double idler_preload = 60000;
// m_idlerPosRel = m_idlerPos;
m_idlerPosRel = ChVector<>(-2.5, 0, 0);
m_chassis->SetFrame_REF_to_abs(ChFrame<>(gear_Csys.pos, gear_Csys.rot));
// Create list of the center location of the rolling elements and their clearance.
// Clearance is a sphere shaped envelope at each center location, where it can
// be guaranteed that the track chain geometry will not penetrate the sphere.
std::vector<ChVector<> > rolling_elem_locs; // w.r.t. chassis ref. frame
std::vector<double> clearance; // 1 per rolling elem
std::vector<ChVector<> > rolling_elem_spin_axis; /// w.r.t. abs. frame
// initialize 1 of each of the following subsystems.
// will use the chassis ref frame to do the transforms, since the TrackSystem
// local ref. frame has same rot (just difference in position)
// NOTE: Gear Init() function moved to after TrackChain() Init
// However, still add rolling element info for gear to list first
// drive sprocket is First added to the lists passed into TrackChain Init()
rolling_elem_locs.push_back(ChVector<>() );
clearance.push_back(m_gear->GetRadius() );
rolling_elem_spin_axis.push_back(m_gear->GetBody()->GetRot().GetZaxis() );
// initialize the support rollers.
for(int r_idx = 0; r_idx < m_num_rollers; r_idx++)
{
ChVector<> roller_loc = m_chassis->GetPos();
roller_loc.y = -1.0;
roller_loc.x -= 0.3*(0 + r_idx);
m_rollers[r_idx]->Initialize(m_chassis,
m_chassis->GetFrame_REF_to_abs(),
ChCoordsys<>(roller_loc, QUNIT) );
// add to the points passed into the track chain
rolling_elem_locs.push_back( roller_loc );
clearance.push_back(m_rollers[r_idx]->GetRadius() );
rolling_elem_spin_axis.push_back( m_rollers[r_idx]->GetBody()->GetRot().GetZaxis() );
}
// init the idler last
m_idlers[0]->Initialize(m_chassis,
m_chassis->GetFrame_REF_to_abs(),
ChCoordsys<>(m_idlerPosRel, Q_from_AngAxis(CH_C_PI, VECT_Z) ),
idler_preload);
// add to the lists passed into the track chain Init()
rolling_elem_locs.push_back(m_idlerPosRel );
clearance.push_back(m_idlers[0]->GetRadius() );
rolling_elem_spin_axis.push_back(m_idlers[0]->GetBody()->GetRot().GetZaxis() );
// when there's only 2 rolling elements, the above locations will repeat
// the first rolling element at the front and end of the vector.
// So, just use the mid-point between the first two rolling elements.
ChVector<> start_pos;
if( clearance.size() <3 )
{
start_pos = (rolling_elem_locs[0] + rolling_elem_locs[1])/2.0;
start_pos.y += (clearance[0] + clearance[1])/2.0;
}
else
{
start_pos = (rolling_elem_locs.front() + rolling_elem_locs.back())/2.0;
start_pos.y += (clearance.front() + clearance.back() )/2.0;
}
start_pos.x += 0.04;
// NOTE: start_pos needs to somewhere on the top length of chain.
// Rolling_elem_locs MUST be ordered from front-most to the rear,
// w.r.t. the chassis x-dir, so chain links created in a clockwise direction.
m_chain->Initialize(m_chassis,
m_chassis->GetFrame_REF_to_abs(),
rolling_elem_locs, clearance,
rolling_elem_spin_axis,
start_pos );
// can set pin friction between adjoining shoes by activing damping on the DOF
// m_chain->Set_pin_friction(2.0); // [N-m-sec] ???
// Now, the gear can be initialized
m_gear->Initialize(m_chassis,
m_chassis->GetFrame_REF_to_abs(),
ChCoordsys<>(),
m_chain->GetShoeBody() );
// initialize the powertrain, drivelines
m_ptrain->Initialize(m_chassis, m_gear->GetAxle() );
// extra idlers?
if(m_num_idlers > 1)
{
// init this idler, and position it so
// it snaps into place
double preload_2 = 120000;
double idler2_xoff = -1.5;
double idler2_yoff = -2.1;
double rotation_ang = CH_C_PI_4;
// get the absolute c-sys of the gear, and set position relative to that point.
ChCoordsys<> idler2_csys(m_gear->GetBody()->GetPos(), m_gear->GetBody()->GetRot());
idler2_csys.pos.x += idler2_xoff;
idler2_csys.pos.y += idler2_yoff;
// rotation should set the -x dir
idler2_csys.rot = Q_from_AngAxis(rotation_ang, VECT_Z);
// set the idler relative to the chassis/gear origin
m_idlers[1]->Initialize(m_chassis,
m_chassis->GetFrame_REF_to_abs(),
idler2_csys,
preload_2);
}
}
ChQuaternion<double> Q_from_AngY(double angleY) {
return Q_from_AngAxis(angleY, VECT_Y);
}
ChQuaternion<double> Q_from_AngZ(double angleZ) {
return Q_from_AngAxis(angleZ, VECT_Z);
}
ChQuaternion<double> Q_from_AngX(double angleX) {
return Q_from_AngAxis(angleX, VECT_X);
}
void ChDoubleWishboneReduced::InitializeSide(VehicleSide side,
std::shared_ptr<ChBodyAuxRef> chassis,
std::shared_ptr<ChBody> tierod_body,
const std::vector<ChVector<> >& points,
double ang_vel) {
std::string suffix = (side == LEFT) ? "_L" : "_R";
// Chassis orientation (expressed in absolute frame)
// Recall that the suspension reference frame is aligned with the chassis.
ChQuaternion<> chassisRot = chassis->GetFrame_REF_to_abs().GetRot();
// Create and initialize spindle body (same orientation as the chassis)
m_spindle[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
m_spindle[side]->SetNameString(m_name + "_spindle" + suffix);
m_spindle[side]->SetPos(points[SPINDLE]);
m_spindle[side]->SetRot(chassisRot);
m_spindle[side]->SetWvel_loc(ChVector<>(0, ang_vel, 0));
m_spindle[side]->SetMass(getSpindleMass());
m_spindle[side]->SetInertiaXX(getSpindleInertia());
chassis->GetSystem()->AddBody(m_spindle[side]);
// Create and initialize upright body (same orientation as the chassis)
m_upright[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
m_upright[side]->SetNameString(m_name + "_upright" + suffix);
m_upright[side]->SetPos(points[UPRIGHT]);
m_upright[side]->SetRot(chassisRot);
m_upright[side]->SetMass(getUprightMass());
m_upright[side]->SetInertiaXX(getUprightInertia());
chassis->GetSystem()->AddBody(m_upright[side]);
// Create and initialize joints
ChCoordsys<> rev_csys(points[SPINDLE], chassisRot * Q_from_AngAxis(CH_C_PI / 2.0, VECT_X));
m_revolute[side] = std::make_shared<ChLinkLockRevolute>();
m_revolute[side]->SetNameString(m_name + "_revolute" + suffix);
m_revolute[side]->Initialize(m_spindle[side], m_upright[side], rev_csys);
chassis->GetSystem()->AddLink(m_revolute[side]);
m_distUCA_F[side] = std::make_shared<ChLinkDistance>();
m_distUCA_F[side]->SetNameString(m_name + "_distUCA_F" + suffix);
m_distUCA_F[side]->Initialize(chassis, m_upright[side], false, points[UCA_F], points[UCA_U]);
chassis->GetSystem()->AddLink(m_distUCA_F[side]);
m_distUCA_B[side] = std::make_shared<ChLinkDistance>();
m_distUCA_B[side]->SetNameString(m_name + "_distUCA_B" + suffix);
m_distUCA_B[side]->Initialize(chassis, m_upright[side], false, points[UCA_B], points[UCA_U]);
chassis->GetSystem()->AddLink(m_distUCA_B[side]);
m_distLCA_F[side] = std::make_shared<ChLinkDistance>();
m_distLCA_F[side]->SetNameString(m_name + "_distLCA_F" + suffix);
m_distLCA_F[side]->Initialize(chassis, m_upright[side], false, points[LCA_F], points[LCA_U]);
chassis->GetSystem()->AddLink(m_distLCA_F[side]);
m_distLCA_B[side] = std::make_shared<ChLinkDistance>();
m_distLCA_B[side]->SetNameString(m_name + "_distLCA_B" + suffix);
m_distLCA_B[side]->Initialize(chassis, m_upright[side], false, points[LCA_B], points[LCA_U]);
chassis->GetSystem()->AddLink(m_distLCA_B[side]);
m_distTierod[side] = std::make_shared<ChLinkDistance>();
m_distTierod[side]->SetNameString(m_name + "_distTierod" + suffix);
m_distTierod[side]->Initialize(tierod_body, m_upright[side], false, points[TIEROD_C], points[TIEROD_U]);
chassis->GetSystem()->AddLink(m_distTierod[side]);
// Create and initialize the spring/damper
m_shock[side] = std::make_shared<ChLinkSpringCB>();
m_shock[side]->SetNameString(m_name + "_shock" + suffix);
m_shock[side]->Initialize(chassis, m_upright[side], false, points[SHOCK_C], points[SHOCK_U]);
m_shock[side]->SetSpringRestLength(getSpringRestLength());
m_shock[side]->RegisterForceFunctor(getShockForceFunctor());
chassis->GetSystem()->AddLink(m_shock[side]);
// Create and initialize the axle shaft and its connection to the spindle.
// Note that the spindle rotates about the Y axis.
m_axle[side] = std::make_shared<ChShaft>();
m_axle[side]->SetNameString(m_name + "_axle" + suffix);
m_axle[side]->SetInertia(getAxleInertia());
m_axle[side]->SetPos_dt(-ang_vel);
chassis->GetSystem()->Add(m_axle[side]);
m_axle_to_spindle[side] = std::make_shared<ChShaftsBody>();
m_axle_to_spindle[side]->SetNameString(m_name + "_axle_to_spindle" + suffix);
m_axle_to_spindle[side]->Initialize(m_axle[side], m_spindle[side], ChVector<>(0, -1, 0));
chassis->GetSystem()->Add(m_axle_to_spindle[side]);
}
