// -----------------------------------------------------------------------------
// Calculate kinematics quantities (slip angle, longitudinal slip, camber angle,
// and toe-in angle using the current state of the associated wheel body.
// -----------------------------------------------------------------------------
void ChTire::CalculateKinematics(double time, const WheelState& state, const ChTerrain& terrain) {
// Wheel normal (expressed in global frame)
ChVector<> wheel_normal = state.rot.GetYaxis();
// Terrain normal at wheel location (expressed in global frame)
ChVector<> Z_dir = terrain.GetNormal(state.pos.x(), state.pos.y());
// Longitudinal (heading) and lateral directions, in the terrain plane
ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
X_dir.Normalize();
ChVector<> Y_dir = Vcross(Z_dir, X_dir);
// Tire reference coordinate system
ChMatrix33<> rot;
rot.Set_A_axis(X_dir, Y_dir, Z_dir);
ChCoordsys<> tire_csys(state.pos, rot.Get_A_quaternion());
// Express wheel linear velocity in tire frame
ChVector<> V = tire_csys.TransformDirectionParentToLocal(state.lin_vel);
// Express wheel normal in tire frame
ChVector<> n = tire_csys.TransformDirectionParentToLocal(wheel_normal);
// Slip angle
double abs_Vx = std::abs(V.x());
double zero_Vx = 1e-4;
m_slip_angle = (abs_Vx > zero_Vx) ? std::atan(V.y() / abs_Vx) : 0;
// Longitudinal slip
m_longitudinal_slip = (abs_Vx > zero_Vx) ? -(V.x() - state.omega * GetRadius()) / abs_Vx : 0;
// Camber angle
m_camber_angle = std::atan2(n.z(), n.y());
}
double ChCollisionUtils::PointTriangleDistance(Vector B,
Vector A1,
Vector A2,
Vector A3,
double& mu,
double& mv,
int& is_into,
Vector& Bprojected) {
// defaults
is_into = 0;
mu = mv = -1;
double mdistance = 10e22;
Vector Dx, Dy, Dz, T1, T1p;
Dx = Vsub(A2, A1);
Dz = Vsub(A3, A1);
Dy = Vcross(Dz, Dx);
double dylen = Vlength(Dy);
if (fabs(dylen) < EPS_TRIDEGEN) // degenerate triangle
return mdistance;
Dy = Vmul(Dy, 1.0 / dylen);
ChMatrix33<> mA;
ChMatrix33<> mAi;
mA.Set_A_axis(Dx, Dy, Dz);
// invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
if (fabs(mA.FastInvert(mAi)) < 0.000001)
return mdistance;
T1 = mAi.Matr_x_Vect(Vsub(B, A1));
T1p = T1;
T1p.y() = 0;
mu = T1.x();
mv = T1.z();
mdistance = -T1.y();
if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu) {
is_into = 1;
Bprojected = Vadd(A1, mA.Matr_x_Vect(T1p));
}
return mdistance;
}
// -----------------------------------------------------------------------------
// Utility function for characterizing the geometric contact between a disc with
// specified center location, normal direction, and radius and the terrain,
// assumed to be specified as a height field (over the x-y domain).
// This function returns false if no contact occurs. Otherwise, it sets the
// contact points on the disc (ptD) and on the terrain (ptT), the normal contact
// direction, and the resulting penetration depth (a positive value).
// -----------------------------------------------------------------------------
bool ChTire::disc_terrain_contact(const ChTerrain& terrain,
const ChVector<>& disc_center,
const ChVector<>& disc_normal,
double disc_radius,
ChCoordsys<>& contact,
double& depth) {
// Find terrain height below disc center. There is no contact if the disc
// center is below the terrain or farther away by more than its radius.
double hc = terrain.GetHeight(disc_center.x(), disc_center.y());
if (disc_center.z() <= hc || disc_center.z() >= hc + disc_radius)
return false;
// Find the lowest point on the disc. There is no contact if the disc is
// (almost) horizontal.
ChVector<> dir1 = Vcross(disc_normal, ChVector<>(0, 0, 1));
double sinTilt2 = dir1.Length2();
if (sinTilt2 < 1e-3)
return false;
// Contact point (lowest point on disc).
ChVector<> ptD = disc_center + disc_radius * Vcross(disc_normal, dir1 / sqrt(sinTilt2));
// Find terrain height at lowest point. No contact if lowest point is above
// the terrain.
double hp = terrain.GetHeight(ptD.x(), ptD.y());
if (ptD.z() > hp)
return false;
// Approximate the terrain with a plane. Define the projection of the lowest
// point onto this plane as the contact point on the terrain.
ChVector<> normal = terrain.GetNormal(ptD.x(), ptD.y());
ChVector<> longitudinal = Vcross(disc_normal, normal);
longitudinal.Normalize();
ChVector<> lateral = Vcross(normal, longitudinal);
ChMatrix33<> rot;
rot.Set_A_axis(longitudinal, lateral, normal);
contact.pos = ptD;
contact.rot = rot.Get_A_quaternion();
depth = Vdot(ChVector<>(0, 0, hp - ptD.z()), normal);
assert(depth > 0);
return true;
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChLinearDamperRWAssembly::Initialize(std::shared_ptr<ChBodyAuxRef> chassis, const ChVector<>& location) {
// Express the suspension reference frame in the absolute coordinate system.
ChFrame<> susp_to_abs(location);
susp_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());
// Transform all points and directions to absolute frame.
std::vector<ChVector<> > points(NUM_POINTS);
for (int i = 0; i < NUM_POINTS; i++) {
ChVector<> rel_pos = GetLocation(static_cast<PointId>(i));
points[i] = susp_to_abs.TransformPointLocalToParent(rel_pos);
}
// Create the trailing arm body. The reference frame of the arm body has its
// x-axis aligned with the line between the arm-chassis connection point and
// the arm-wheel connection point.
ChVector<> y_dir = susp_to_abs.GetA().Get_A_Yaxis();
ChVector<> u = susp_to_abs.GetPos() - points[ARM_CHASSIS];
u.Normalize();
ChVector<> w = Vcross(u, y_dir);
w.Normalize();
ChVector<> v = Vcross(w, u);
ChMatrix33<> rot;
rot.Set_A_axis(u, v, w);
m_arm = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
m_arm->SetNameString(m_name + "_arm");
m_arm->SetPos(points[ARM]);
m_arm->SetRot(rot);
m_arm->SetMass(GetArmMass());
m_arm->SetInertiaXX(GetArmInertia());
chassis->GetSystem()->AddBody(m_arm);
// Cache points and directions for arm visualization (expressed in the arm frame)
m_pO = m_arm->TransformPointParentToLocal(susp_to_abs.GetPos());
m_pA = m_arm->TransformPointParentToLocal(points[ARM]);
m_pAW = m_arm->TransformPointParentToLocal(points[ARM_WHEEL]);
m_pAC = m_arm->TransformPointParentToLocal(points[ARM_CHASSIS]);
m_pAS = m_arm->TransformPointParentToLocal(points[SHOCK_A]);
m_dY = m_arm->TransformDirectionParentToLocal(y_dir);
// Create and initialize the revolute joint between arm and chassis.
// The axis of rotation is the y axis of the suspension reference frame.
m_revolute = std::make_shared<ChLinkLockRevolute>();
m_revolute->SetNameString(m_name + "_revolute");
m_revolute->Initialize(chassis, m_arm,
ChCoordsys<>(points[ARM_CHASSIS], susp_to_abs.GetRot() * Q_from_AngX(CH_C_PI_2)));
chassis->GetSystem()->AddLink(m_revolute);
// Create and initialize the rotational spring torque element.
m_spring = std::make_shared<ChLinkRotSpringCB>();
m_spring->SetNameString(m_name + "_spring");
m_spring->Initialize(chassis, m_arm, ChCoordsys<>(points[ARM_CHASSIS], susp_to_abs.GetRot() * Q_from_AngX(CH_C_PI_2)));
m_spring->RegisterTorqueFunctor(GetSpringTorqueFunctor());
chassis->GetSystem()->AddLink(m_spring);
// Create and initialize the translational shock force element.
if (m_has_shock) {
m_shock = std::make_shared<ChLinkSpringCB>();
m_shock->SetNameString(m_name + "_shock");
m_shock->Initialize(chassis, m_arm, false, points[SHOCK_C], points[SHOCK_A]);
m_shock->RegisterForceFunctor(GetShockForceFunctor());
chassis->GetSystem()->AddLink(m_shock);
}
// Invoke the base class implementation. This initializes the associated road wheel.
// Note: we must call this here, after the m_arm body is created.
ChRoadWheelAssembly::Initialize(chassis, location);
}
void ChLinkPulley::UpdateTime (double mytime)
{
// First, inherit to parent class
ChLinkLock::UpdateTime(mytime);
ChFrame<double> abs_shaft1;
ChFrame<double> abs_shaft2;
((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);
ChVector<> dcc_w = Vsub(Get_shaft_pos2(),
Get_shaft_pos1());
// compute actual rotation of the two wheels (relative to truss).
Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(dcc_w);
md1.z = 0; md1 = Vnorm (md1);
Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(dcc_w);
md2.z = 0; md2 = Vnorm (md2);
double periodic_a1 = ChAtan2(md1.x, md1.y);
double periodic_a2 = ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = floor (old_a1 / CH_C_2PI);
double turns_a2 = floor (old_a2 / CH_C_2PI);
double a1U = turns_a1 * CH_C_2PI + periodic_a1 + CH_C_2PI;
double a1M = turns_a1 * CH_C_2PI + periodic_a1;
double a1L = turns_a1 * CH_C_2PI + periodic_a1 - CH_C_2PI;
a1 = a1M;
if (fabs(a1U - old_a1) < fabs(a1M - old_a1))
a1 = a1U;
if (fabs(a1L - a1) < fabs(a1M - a1))
a1 = a1L;
double a2U = turns_a2 * CH_C_2PI + periodic_a2 + CH_C_2PI;
double a2M = turns_a2 * CH_C_2PI + periodic_a2;
double a2L = turns_a2 * CH_C_2PI + periodic_a2 - CH_C_2PI;
a2 = a2M;
if (fabs(a2U - old_a2) < fabs(a2M - old_a2))
a2 = a2U;
if (fabs(a2L - a2) < fabs(a2M - a2))
a2 = a2L;
// correct marker positions if phasing is not correct
double m_delta =0;
if (this->checkphase)
{
double realtau = tau;
//if (this->epicyclic)
// realtau = -tau;
m_delta = a1 - phase - (a2/realtau);
if (m_delta> CH_C_PI) m_delta -= (CH_C_2PI); // range -180..+180 is better than 0...360
if (m_delta> (CH_C_PI/4.0)) m_delta = (CH_C_PI/4.0); // phase correction only in +/- 45°
if (m_delta<-(CH_C_PI/4.0)) m_delta =-(CH_C_PI/4.0);
//***TODO***
}
// Move markers 1 and 2 to align them as pulley ends
ChVector<> d21_w = dcc_w - Get_shaft_dir1()* Vdot (Get_shaft_dir1(), dcc_w);
ChVector<> D21_w = Vnorm(d21_w);
this->shaft_dist = d21_w.Length();
ChVector<> U1_w = Vcross(Get_shaft_dir1(), D21_w);
double gamma1 = acos( (r1-r2) / shaft_dist);
ChVector<> Ru_w = D21_w*cos(gamma1) + U1_w*sin(gamma1);
ChVector<> Rl_w = D21_w*cos(gamma1) - U1_w*sin(gamma1);
this->belt_up1 = Get_shaft_pos1()+ Ru_w*r1;
this->belt_low1 = Get_shaft_pos1()+ Rl_w*r1;
this->belt_up2 = Get_shaft_pos1()+ d21_w + Ru_w*r2;
this->belt_low2 = Get_shaft_pos1()+ d21_w + Rl_w*r2;
// marker alignment
ChMatrix33<> maU;
ChMatrix33<> maL;
ChVector<> Dxu = Vnorm(belt_up2 - belt_up1);
ChVector<> Dyu = Ru_w;
ChVector<> Dzu = Vnorm (Vcross(Dxu, Dyu));
Dyu = Vnorm (Vcross(Dzu, Dxu));
maU.Set_A_axis(Dxu,Dyu,Dzu);
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
marker1->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
ChCoordsys<> newmarkpos;
// move marker1 in proper positions
newmarkpos.pos = this->belt_up1;
newmarkpos.rot = maU.Get_A_quaternion();
marker1->Impose_Abs_Coord(newmarkpos); //move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = this->belt_up2;
newmarkpos.rot = maU.Get_A_quaternion();
marker2->Impose_Abs_Coord(newmarkpos); //move marker2 into teeth position
double phase_correction_up = m_delta*r1;
double phase_correction_low = - phase_correction_up;
double hU = Vlenght(belt_up2- belt_up1) + phase_correction_up;
double hL = Vlenght(belt_low2- belt_low1) + phase_correction_low;
// imposed relative positions/speeds
deltaC.pos = ChVector<>(-hU, 0, 0);
deltaC_dt.pos = VNULL;
deltaC_dtdt.pos = VNULL;
deltaC.rot = QUNIT; // no relative rotations imposed!
deltaC_dt.rot = QNULL;
deltaC_dtdt.rot = QNULL;
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPitmanArm::Initialize(std::shared_ptr<ChBodyAuxRef> chassis,
const ChVector<>& location,
const ChQuaternion<>& rotation) {
m_position = ChCoordsys<>(location, rotation);
// 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();
// Express the steering reference frame in the absolute coordinate system.
ChFrame<> steering_to_abs(location, rotation);
steering_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());
// Transform all points and directions to absolute frame.
std::vector<ChVector<>> points(NUM_POINTS);
std::vector<ChVector<>> dirs(NUM_DIRS);
for (int i = 0; i < NUM_POINTS; i++) {
ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
points[i] = steering_to_abs.TransformPointLocalToParent(rel_pos);
}
for (int i = 0; i < NUM_DIRS; i++) {
ChVector<> rel_dir = getDirection(static_cast<DirectionId>(i));
dirs[i] = steering_to_abs.TransformDirectionLocalToParent(rel_dir);
}
// Unit vectors for orientation matrices.
ChVector<> u;
ChVector<> v;
ChVector<> w;
ChMatrix33<> rot;
// Create and initialize the steering link body
m_link = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
m_link->SetNameString(m_name + "_link");
m_link->SetPos(points[STEERINGLINK]);
m_link->SetRot(steering_to_abs.GetRot());
m_link->SetMass(getSteeringLinkMass());
if (m_vehicle_frame_inertia) {
ChMatrix33<> inertia = TransformInertiaMatrix(getSteeringLinkInertiaMoments(), getSteeringLinkInertiaProducts(),
chassisRot, steering_to_abs.GetRot());
m_link->SetInertia(inertia);
} else {
m_link->SetInertiaXX(getSteeringLinkInertiaMoments());
m_link->SetInertiaXY(getSteeringLinkInertiaProducts());
}
chassis->GetSystem()->AddBody(m_link);
m_pP = m_link->TransformPointParentToLocal(points[UNIV]);
m_pI = m_link->TransformPointParentToLocal(points[REVSPH_S]);
m_pTP = m_link->TransformPointParentToLocal(points[TIEROD_PA]);
m_pTI = m_link->TransformPointParentToLocal(points[TIEROD_IA]);
// Create and initialize the Pitman arm body
m_arm = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
m_arm->SetNameString(m_name + "_arm");
m_arm->SetPos(points[PITMANARM]);
m_arm->SetRot(steering_to_abs.GetRot());
m_arm->SetMass(getPitmanArmMass());
if (m_vehicle_frame_inertia) {
ChMatrix33<> inertia = TransformInertiaMatrix(getPitmanArmInertiaMoments(), getPitmanArmInertiaProducts(),
chassisRot, steering_to_abs.GetRot());
m_arm->SetInertia(inertia);
} else {
m_arm->SetInertiaXX(getPitmanArmInertiaMoments());
m_arm->SetInertiaXY(getPitmanArmInertiaProducts());
}
chassis->GetSystem()->AddBody(m_arm);
// Cache points for arm visualization (expressed in the arm frame)
m_pC = m_arm->TransformPointParentToLocal(points[REV]);
m_pL = m_arm->TransformPointParentToLocal(points[UNIV]);
// Create and initialize the revolute joint between chassis and Pitman arm.
// Note that this is modeled as a ChLinkEngine to allow driving it with
// imposed rotation (steering input).
// The z direction of the joint orientation matrix is dirs[REV_AXIS], assumed
// to be a unit vector.
u = points[PITMANARM] - points[REV];
v = Vcross(dirs[REV_AXIS], u);
v.Normalize();
u = Vcross(v, dirs[REV_AXIS]);
rot.Set_A_axis(u, v, dirs[REV_AXIS]);
m_revolute = std::make_shared<ChLinkMotorRotationAngle>();
m_revolute->SetNameString(m_name + "_revolute");
m_revolute->Initialize(chassis, m_arm, ChFrame<>(points[REV], rot.Get_A_quaternion()));
auto motor_fun = std::make_shared<ChFunction_Setpoint>();
m_revolute->SetAngleFunction(motor_fun);
chassis->GetSystem()->AddLink(m_revolute);
// Create and initialize the universal joint between the Pitman arm and steering link.
// The x and y directions of the joint orientation matrix are given by
// dirs[UNIV_AXIS_ARM] and dirs[UNIV_AXIS_LINK], assumed to be unit vectors
// and orthogonal.
w = Vcross(dirs[UNIV_AXIS_ARM], dirs[UNIV_AXIS_LINK]);
rot.Set_A_axis(dirs[UNIV_AXIS_ARM], dirs[UNIV_AXIS_LINK], w);
m_universal = std::make_shared<ChLinkUniversal>();
m_universal->SetNameString(m_name + "_universal");
m_universal->Initialize(m_arm, m_link, ChFrame<>(points[UNIV], rot.Get_A_quaternion()));
chassis->GetSystem()->AddLink(m_universal);
// Create and initialize the revolute-spherical joint (massless idler arm).
// The length of the idler arm is the distance between the two hardpoints.
// The z direction of the revolute joint orientation matrix is
// dirs[REVSPH_AXIS], assumed to be a unit vector.
double distance = (points[REVSPH_S] - points[REVSPH_R]).Length();
u = points[REVSPH_S] - points[REVSPH_R];
v = Vcross(dirs[REVSPH_AXIS], u);
v.Normalize();
u = Vcross(v, dirs[REVSPH_AXIS]);
rot.Set_A_axis(u, v, dirs[REVSPH_AXIS]);
m_revsph = std::make_shared<ChLinkRevoluteSpherical>();
m_revsph->SetNameString(m_name + "_revsph");
m_revsph->Initialize(chassis, m_link, ChCoordsys<>(points[REVSPH_R], rot.Get_A_quaternion()), distance);
chassis->GetSystem()->AddLink(m_revsph);
}
