1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Radu Serban, Justin Madsen
13 // =============================================================================
14 //
15 // Base class for a vehicle wheel.
16 // A wheel subsystem does not own a body. Instead, when attached to the spindle
17 // of a suspension subsystem, the wheel's mass properties are used to update
18 // those of the spindle body owned by the suspension.
19 // A concrete wheel subsystem can optionally carry its own visualization assets
20 // (which are associated with the suspension's spindle body).
21 //
22 // =============================================================================
23
24 #include <algorithm>
25
26 #include "chrono/core/ChGlobal.h"
27 #include "chrono/assets/ChTexture.h"
28
29 #include "chrono_vehicle/ChVehicleModelData.h"
30 #include "chrono_vehicle/wheeled_vehicle/ChWheel.h"
31 #include "chrono_vehicle/wheeled_vehicle/ChTire.h"
32
33 #include "chrono_thirdparty/filesystem/path.h"
34
35 namespace chrono {
36 namespace vehicle {
37
ChWheel(const std::string & name)38 ChWheel::ChWheel(const std::string& name) : ChPart(name), m_offset(0), m_side(LEFT) {}
39
40 // Initialize this wheel by associating it to the specified suspension spindle body.
41 // Increment the mass and inertia of the spindle body to account for the wheel mass and inertia.
Initialize(std::shared_ptr<ChBody> spindle,VehicleSide side,double offset)42 void ChWheel::Initialize(std::shared_ptr<ChBody> spindle, VehicleSide side, double offset) {
43 m_spindle = spindle;
44 m_side = side;
45 m_offset = (side == LEFT) ? offset : -offset;
46
47 //// RADU
48 //// Todo: Properly account for offset in adjusting inertia.
49 //// This requires changing the spindle to a ChBodyAuxRef.
50 m_spindle->SetMass(m_spindle->GetMass() + GetMass());
51 m_spindle->SetInertiaXX(m_spindle->GetInertiaXX() + GetInertia());
52 }
53
Synchronize()54 void ChWheel::Synchronize() {
55 if (!m_tire)
56 return;
57 auto tire_force = m_tire->GetTireForce();
58 m_spindle->Accumulate_force(tire_force.force, tire_force.point, false);
59 m_spindle->Accumulate_torque(tire_force.moment, false);
60 }
61
GetPos() const62 ChVector<> ChWheel::GetPos() const {
63 return m_spindle->TransformPointLocalToParent(ChVector<>(0, m_offset, 0));
64 }
65
GetState() const66 WheelState ChWheel::GetState() const {
67 WheelState state;
68
69 ChFrameMoving<> wheel_loc(ChVector<>(0, m_offset, 0), QUNIT);
70 ChFrameMoving<> wheel_abs;
71 m_spindle->TransformLocalToParent(wheel_loc, wheel_abs);
72 state.pos = wheel_abs.GetPos();
73 state.rot = wheel_abs.GetRot();
74 state.lin_vel = wheel_abs.GetPos_dt();
75 state.ang_vel = wheel_abs.GetWvel_par();
76
77 ChVector<> ang_vel_loc = state.rot.RotateBack(state.ang_vel);
78 state.omega = ang_vel_loc.y();
79
80 return state;
81 }
82
83 // -----------------------------------------------------------------------------
84 // -----------------------------------------------------------------------------
AddVisualizationAssets(VisualizationType vis)85 void ChWheel::AddVisualizationAssets(VisualizationType vis) {
86 if (vis == VisualizationType::NONE)
87 return;
88
89 if (vis == VisualizationType::MESH && !m_vis_mesh_file.empty()) {
90 ChQuaternion<> rot = (m_side == VehicleSide::LEFT) ? Q_from_AngZ(0) : Q_from_AngZ(CH_C_PI);
91 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
92 trimesh->LoadWavefrontMesh(vehicle::GetDataFile(m_vis_mesh_file), false, false);
93 trimesh->Transform(ChVector<>(0, m_offset, 0), ChMatrix33<>(rot));
94 m_trimesh_shape = chrono_types::make_shared<ChTriangleMeshShape>();
95 m_trimesh_shape->Pos = ChVector<>(0, m_offset, 0);
96 m_trimesh_shape->Rot = ChMatrix33<>(rot);
97 m_trimesh_shape->SetMesh(trimesh);
98 m_trimesh_shape->SetName(filesystem::path(m_vis_mesh_file).stem());
99 m_trimesh_shape->SetStatic(true);
100 m_spindle->AddAsset(m_trimesh_shape);
101 return;
102 }
103
104 if (GetRadius() == 0 || GetWidth() == 0)
105 return;
106
107 m_cyl_shape = chrono_types::make_shared<ChCylinderShape>();
108 m_cyl_shape->GetCylinderGeometry().rad = GetRadius();
109 m_cyl_shape->GetCylinderGeometry().p1 = ChVector<>(0, m_offset + GetWidth() / 2, 0);
110 m_cyl_shape->GetCylinderGeometry().p2 = ChVector<>(0, m_offset - GetWidth() / 2, 0);
111 m_spindle->AddAsset(m_cyl_shape);
112 }
113
RemoveVisualizationAssets()114 void ChWheel::RemoveVisualizationAssets() {
115 // Make sure we only remove the assets added by ChWheel::AddVisualizationAssets.
116 // This is important for the ChWheel object because a tire may add its own assets
117 // to the same body (the spindle).
118 auto& assets = m_spindle->GetAssets();
119
120 auto it_cyl = std::find(assets.begin(), assets.end(), m_cyl_shape);
121 if (it_cyl != assets.end())
122 assets.erase(it_cyl);
123
124 auto it_mesh = std::find(assets.begin(), assets.end(), m_trimesh_shape);
125 if (it_mesh != assets.end())
126 assets.erase(it_mesh);
127 }
128
129 } // end namespace vehicle
130 } // end namespace chrono
131