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