int ChPathSteeringControllerXT::CalcCurvatureCode(ChVector<>& a, ChVector<>& b) {
// a[] is a unit vector pointing to the left vehicle side
// b[] is a unit vector pointing to the instantanous curve center
a.z() = 0;
a.Normalize();
b.z() = 0;
b.Normalize();
// In a left turn the distance between the two points will be nearly zero
// in a right turn the distance will be around 2, at least > 1
ChVector<> ab = a - b;
double ltest = ab.Length();
// What is a straight line? We define a threshold radius R_threshold
// if the actual curvature is greater than 1/R_treshold, we are in a curve
// otherwise we take this point as part of a straight line
// m_pcurvature is always >= 0
if(m_pcurvature <= 1.0/m_R_threshold) {
return 0; // -> straight line
}
if(ltest < 1.0) {
return 1; // left bending curve
}
return -1; // right bending curve
}
// Get the quaternion from a source vector and a destination vector which specifies
// the rotation from one to the other. The vectors do not need to be normalized.
ChQuaternion<double> Q_from_Vect_to_Vect(const ChVector<double>& fr_vect, const ChVector<double>& to_vect) {
const double ANGLE_TOLERANCE = 1e-6;
ChQuaternion<double> quat;
double halfang;
double sinhalf;
ChVector<double> axis;
double lenXlen = fr_vect.Length() * to_vect.Length();
axis = fr_vect % to_vect;
double sinangle = ChClamp(axis.Length() / lenXlen, -1.0, +1.0);
double cosangle = ChClamp(fr_vect ^ to_vect / lenXlen, -1.0, +1.0);
// Consider three cases: Parallel, Opposite, non-collinear
if (std::abs(sinangle) == 0.0 && cosangle > 0) {
// fr_vect & to_vect are parallel
quat.e0() = 1.0;
quat.e1() = 0.0;
quat.e2() = 0.0;
quat.e3() = 0.0;
} else if (std::abs(sinangle) < ANGLE_TOLERANCE && cosangle < 0) {
// fr_vect & to_vect are opposite, i.e. ~180 deg apart
axis = fr_vect.GetOrthogonalVector() + (-to_vect).GetOrthogonalVector();
axis.Normalize();
quat.e0() = 0.0;
quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
} else {
// fr_vect & to_vect are not co-linear case
axis.Normalize();
halfang = 0.5 * ChAtan2(sinangle, cosangle);
sinhalf = sin(halfang);
quat.e0() = cos(halfang);
quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
}
return (quat);
}
double ChPathSteeringControllerXT::CalcHeadingError(ChVector<>& a, ChVector<>& b) {
double ang = 0.0;
// test for velocity > 0
m_vel.z() = 0;
m_vel.Normalize();
double speed = m_vel.Length();
if(speed < 1) {
// vehicle is standing still, we take the chassis orientation
a.z() = 0;
b.z() = 0;
a.Normalize();
b.Normalize();
} else {
// vehicle is running, we take the {x,y} velocity vector
a = m_vel;
b.z() = 0;
b.Normalize();
}
// it might happen to cruise against the path definition (end->begin instead of begin->end),
// then the the tangent points backwards to driving direction
// the distance |ab| is > 1 then
ChVector<> ab = a - b;
double ltest = ab.Length();
ChVector<> vpc;
if(ltest < 1) {
vpc = Vcross(a,b);
} else {
vpc = Vcross(a,-b);
}
ang = std::asin(vpc.z());
return ang;
}
// -----------------------------------------------------------------------------
// 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 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);
}
