void ChLinkDistance::Update (double mytime)
{
// Inherit time changes of parent class (ChLink), basically doing nothing :)
ChLink::UpdateTime(mytime);
// compute jacobians
ChVector<> AbsDist = Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2);
curr_dist = AbsDist.Length();
ChVector<> D2abs = Vnorm(AbsDist);
ChVector<> D2relB = Body2->Dir_World2Body(&D2abs);
ChVector<> D2relA = Body1->Dir_World2Body(&D2abs);
ChVector<> CqAx = D2abs;
ChVector<> CqBx = -D2abs;
ChVector<> CqAr = -Vcross(D2relA,pos1);
ChVector<> CqBr = Vcross(D2relB,pos2);
Cx.Get_Cq_a()->ElementN(0)=(float)CqAx.x;
Cx.Get_Cq_a()->ElementN(1)=(float)CqAx.y;
Cx.Get_Cq_a()->ElementN(2)=(float)CqAx.z;
Cx.Get_Cq_a()->ElementN(3)=(float)CqAr.x;
Cx.Get_Cq_a()->ElementN(4)=(float)CqAr.y;
Cx.Get_Cq_a()->ElementN(5)=(float)CqAr.z;
Cx.Get_Cq_b()->ElementN(0)=(float)CqBx.x;
Cx.Get_Cq_b()->ElementN(1)=(float)CqBx.y;
Cx.Get_Cq_b()->ElementN(2)=(float)CqBx.z;
Cx.Get_Cq_b()->ElementN(3)=(float)CqBr.x;
Cx.Get_Cq_b()->ElementN(4)=(float)CqBr.y;
Cx.Get_Cq_b()->ElementN(5)=(float)CqBr.z;
//***TO DO*** C_dt? C_dtdt? (may be never used..)
}
// -----------------------------------------------------------------------------
// 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());
}
void ChLinkDistance::Update(double mytime, bool update_assets) {
// Inherit time changes of parent class (ChLink), basically doing nothing :)
ChLink::Update(mytime, update_assets);
// compute jacobians
ChVector<> AbsDist = Body1->TransformPointLocalToParent(pos1) - Body2->TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
ChVector<> D2abs = Vnorm(AbsDist);
ChVector<> D2relB = Body2->TransformDirectionParentToLocal(D2abs);
ChVector<> D2relA = Body1->TransformDirectionParentToLocal(D2abs);
ChVector<> CqAx = D2abs;
ChVector<> CqBx = -D2abs;
ChVector<> CqAr = -Vcross(D2relA, pos1);
ChVector<> CqBr = Vcross(D2relB, pos2);
Cx.Get_Cq_a()->ElementN(0) = CqAx.x();
Cx.Get_Cq_a()->ElementN(1) = CqAx.y();
Cx.Get_Cq_a()->ElementN(2) = CqAx.z();
Cx.Get_Cq_a()->ElementN(3) = CqAr.x();
Cx.Get_Cq_a()->ElementN(4) = CqAr.y();
Cx.Get_Cq_a()->ElementN(5) = CqAr.z();
Cx.Get_Cq_b()->ElementN(0) = CqBx.x();
Cx.Get_Cq_b()->ElementN(1) = CqBx.y();
Cx.Get_Cq_b()->ElementN(2) = CqBx.z();
Cx.Get_Cq_b()->ElementN(3) = CqBr.x();
Cx.Get_Cq_b()->ElementN(4) = CqBr.y();
Cx.Get_Cq_b()->ElementN(5) = CqBr.z();
//***TO DO*** C_dt? C_dtdt? (may be never used..)
}
void ChBodyFrame::To_abs_forcetorque(const ChVector<>& force,
const ChVector<>& appl_point,
bool local,
ChVector<>& resultforce,
ChVector<>& resulttorque) {
if (local) {
// local space
ChVector<> mforce_abs = TransformDirectionLocalToParent(force);
resultforce = mforce_abs;
resulttorque = Vcross(TransformDirectionLocalToParent(appl_point), mforce_abs);
} else {
// absolute space
resultforce = force;
resulttorque = Vcross(Vsub(appl_point, coord.pos), force);
}
}
bool DegenerateTriangle(Vector Dx, Vector Dy) {
Vector vcr;
vcr = Vcross(Dx, Dy);
if (fabs(vcr.x()) < EPS_TRIDEGEN && fabs(vcr.y()) < EPS_TRIDEGEN && fabs(vcr.z()) < EPS_TRIDEGEN)
return true;
return false;
}
// -----------------------------------------------------------------------------
// 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;
}
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;
}
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;
}
double ChCollisionUtils::PointLineDistance(Vector p, Vector dA, Vector dB, double& mu, int& is_insegment) {
mu = -1.0;
is_insegment = 0;
double mdist = 10e34;
Vector vseg = Vsub(dB, dA);
Vector vdir = Vnorm(vseg);
Vector vray = Vsub(p, dA);
mdist = Vlength(Vcross(vray, vdir));
mu = Vdot(vray, vdir) / Vlength(vseg);
if ((mu >= 0) && (mu <= 1.0))
is_insegment = 1;
return mdist;
}
double ChSteeringController::Advance(const ChVehicle& vehicle, double step) {
// Calculate current "sentinel" location. This is a point at the look-ahead
// distance in front of the vehicle.
m_sentinel = vehicle.GetChassisBody()->GetFrame_REF_to_abs().TransformPointLocalToParent(ChVector<>(m_dist, 0, 0));
// Calculate current "target" location.
CalcTargetLocation();
// If data collection is enabled, append current target and sentinel locations.
if (m_collect) {
*m_csv << vehicle.GetChTime() << m_target << m_sentinel << std::endl;
}
// The "error" vector is the projection onto the horizontal plane (z=0) of
// the vector between sentinel and target.
ChVector<> err_vec = m_target - m_sentinel;
err_vec.z() = 0;
// Calculate the sign of the angle between the projections of the sentinel
// vector and the target vector (with origin at vehicle location).
ChVector<> sentinel_vec = m_sentinel - vehicle.GetVehiclePos();
sentinel_vec.z() = 0;
ChVector<> target_vec = m_target - vehicle.GetVehiclePos();
target_vec.z() = 0;
double temp = Vdot(Vcross(sentinel_vec, target_vec), ChVector<>(0, 0, 1));
// Calculate current error (magnitude).
double err = ChSignum(temp) * err_vec.Length();
// Estimate error derivative (backward FD approximation).
m_errd = (err - m_err) / step;
// Calculate current error integral (trapezoidal rule).
m_erri += (err + m_err) * step / 2;
// Cache new error
m_err = err;
// Return PID output (steering value)
return m_Kp * m_err + m_Ki * m_erri + m_Kd * m_errd;
}
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 ChLinkGear::UpdateTime (double mytime)
{
// First, inherit to parent class
ChLinkLock::UpdateTime(mytime);
// Move markers 1 and 2 to align them as gear teeth
ChMatrix33<> ma1;
ChMatrix33<> ma2;
ChMatrix33<> mrotma;
ChMatrix33<> marot_beta;
Vector mx;
Vector my;
Vector mz;
Vector mr;
Vector mmark1;
Vector mmark2;
Vector lastX;
Vector vrota;
Coordsys newmarkpos;
ChFrame<double> abs_shaft1;
ChFrame<double> abs_shaft2;
((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);
Vector vbdist = Vsub(Get_shaft_pos1(),
Get_shaft_pos2());
Vector Trad1 = Vnorm(Vcross(Get_shaft_dir1(), Vnorm(Vcross(Get_shaft_dir1(),vbdist))));
Vector Trad2 = Vnorm(Vcross(Vnorm(Vcross(Get_shaft_dir2(),vbdist)), Get_shaft_dir2()));
double dist = Vlenght(vbdist);
// compute actual rotation of the two wheels (relative to truss).
Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(-vbdist);
md1.z = 0; md1 = Vnorm (md1);
Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(-vbdist);
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;
// compute new markers coordsystem alignment
my = Vnorm (vbdist);
mz = Get_shaft_dir1();
mx = Vnorm(Vcross (my, mz));
mr = Vnorm(Vcross (mz, mx));
mz = Vnorm(Vcross (mx, my));
ChVector<> mz2, mx2, mr2, my2;
my2 = my;
mz2 = Get_shaft_dir2();
mx2 = Vnorm(Vcross (my2, mz2));
mr2 = Vnorm(Vcross (mz2, mx2));
ma1.Set_A_axis(mx,my,mz);
// rotate csys because of beta
vrota.x = 0.0; vrota.y = this->beta; vrota.z = 0.0;
mrotma.Set_A_Rxyz(vrota);
marot_beta.MatrMultiply(ma1, mrotma);
// rotate csys because of alpha
vrota.x = 0.0; vrota.y = 0.0; vrota.z = this->alpha;
if (react_force.x < 0) vrota.z = this->alpha;
else vrota.z = -this->alpha;
mrotma.Set_A_Rxyz(vrota);
ma1.MatrMultiply(marot_beta, mrotma);
ma2.CopyFromMatrix(ma1);
// is a bevel gear?
double be = acos(Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
bool is_bevel= true;
if (fabs( Vdot(Get_shaft_dir1(), Get_shaft_dir2()) )>0.96)
is_bevel = false;
// compute wheel radii
// so that:
// w2 = - tau * w1
if (!is_bevel)
{
double pardist = Vdot(mr, vbdist);
double inv_tau = 1.0/tau;
if (!this->epicyclic)
{
r2 = pardist - pardist / (inv_tau + 1.0);
}
else
{
r2 = pardist - (tau * pardist)/(tau-1.0);
}
r1 = r2*tau; }
else
{
double gamma2;
if (!this->epicyclic)
{
gamma2 = be/(tau + 1.0);
}
else
{
gamma2 = be/(-tau + 1.0);
}
double al = CH_C_PI - acos (Vdot(Get_shaft_dir2(), my));
double te = CH_C_PI - al - be;
double fd = sin(te) * (dist/sin(be));
r2 = fd * tan(gamma2);
r1 = r2*tau;
}
// compute markers positions, supposing they
// stay on the ideal wheel contact point
mmark1 = Vadd(Get_shaft_pos2(), Vmul(mr2, r2));
mmark2 = mmark1;
contact_pt = mmark1;
// correct marker 1 position if phasing is not correct
if (this->checkphase)
{
double realtau = tau;
if (this->epicyclic)
realtau = -tau;
double m_delta;
m_delta = - (a2/realtau) - a1 - phase;
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);
vrota.x = vrota.y = 0.0; vrota.z = - m_delta;
mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct
mmark1 = abs_shaft1.GetA()->MatrT_x_Vect(Vsub(mmark1, Get_shaft_pos1() ));
mmark1 = mrotma.Matr_x_Vect(mmark1);
mmark1 = Vadd (abs_shaft1.GetA()->Matr_x_Vect(mmark1), Get_shaft_pos1() );
}
// Move Shaft 1 along its direction if not aligned to wheel
double offset = Vdot (this->Get_shaft_dir1(), (contact_pt - this->Get_shaft_pos1()) );
ChVector<> moff = this->Get_shaft_dir1() * offset;
if (fabs (offset) > 0.0001)
this->local_shaft1.SetPos( local_shaft1.GetPos() + Body1->Dir_World2Body(&moff) );
// ! 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);
// move marker1 in proper positions
newmarkpos.pos = mmark1;
newmarkpos.rot = ma1.Get_A_quaternion();
marker1->Impose_Abs_Coord(newmarkpos); //move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = mmark2;
newmarkpos.rot = ma2.Get_A_quaternion();
marker2->Impose_Abs_Coord(newmarkpos); //move marker2 into teeth position
// imposed relative positions/speeds
deltaC.pos = VNULL;
deltaC_dt.pos = VNULL;
deltaC_dtdt.pos = VNULL;
deltaC.rot = QUNIT; // no relative rotations imposed!
deltaC_dt.rot = QNULL;
deltaC_dtdt.rot = QNULL;
}
void Viewer::computeExtrusion(int nOrientation,
float *orientation,
int nScale,
float *scale,
int nCrossSection,
float *crossSection,
int nSpine,
float *spine,
float *c, // OUT: coordinates
float *tc, // OUT: texture coords
int *faces) // OUT: face list
{
int i, j, ci;
// Xscp, Yscp, Zscp- columns of xform matrix to align cross section
// with spine segments.
float Xscp[3] = { 1.0, 0.0, 0.0};
float Yscp[3] = { 0.0, 1.0, 0.0};
float Zscp[3] = { 0.0, 0.0, 1.0};
float lastZ[3];
// Is the spine a closed curve (last pt == first pt)?
bool spineClosed = (FPZERO(spine[ 3*(nSpine-1)+0 ] - spine[0]) &&
FPZERO(spine[ 3*(nSpine-1)+1 ] - spine[1]) &&
FPZERO(spine[ 3*(nSpine-1)+2 ] - spine[2]));
// Is the spine a straight line?
bool spineStraight = true;
for (i = 1; i < nSpine-1; ++i)
{
float v1[3], v2[3];
v1[0] = spine[3*(i-1)+0] - spine[3*(i)+0];
v1[1] = spine[3*(i-1)+1] - spine[3*(i)+1];
v1[2] = spine[3*(i-1)+2] - spine[3*(i)+2];
v2[0] = spine[3*(i+1)+0] - spine[3*(i)+0];
v2[1] = spine[3*(i+1)+1] - spine[3*(i)+1];
v2[2] = spine[3*(i+1)+2] - spine[3*(i)+2];
Vcross(v1, v2, v1);
if (Vlength(v1) != 0.0)
{
spineStraight = false;
Vnorm( v1 );
Vset( lastZ, v1 );
break;
}
}
// If the spine is a straight line, compute a constant SCP xform
if (spineStraight)
{
float V1[3] = { 0.0, 1.0, 0.0}, V2[3], V3[3];
V2[0] = spine[3*(nSpine-1)+0] - spine[0];
V2[1] = spine[3*(nSpine-1)+1] - spine[1];
V2[2] = spine[3*(nSpine-1)+2] - spine[2];
Vcross( V3, V2, V1 );
float len = (float)Vlength(V3);
if (len != 0.0) // Not aligned with Y axis
{
Vscale(V3, 1.0f/len);
float orient[4]; // Axis/angle
Vset(orient, V3);
orient[3] = acos(Vdot(V1,V2));
double scp[4][4]; // xform matrix
Mrotation( scp, orient );
for (int k=0; k<3; ++k) {
Xscp[k] = (float)scp[0][k];
Yscp[k] = (float)scp[1][k];
Zscp[k] = (float)scp[2][k];
}
}
}
// Orientation matrix
double om[4][4];
if (nOrientation == 1 && ! FPZERO(orientation[3]) )
Mrotation( om, orientation );
// Compute coordinates, texture coordinates:
for (i = 0, ci = 0; i < nSpine; ++i, ci+=nCrossSection) {
// Scale cross section
for (j = 0; j < nCrossSection; ++j) {
c[3*(ci+j)+0] = scale[0] * crossSection[ 2*j ];
c[3*(ci+j)+1] = 0.0;
c[3*(ci+j)+2] = scale[1] * crossSection[ 2*j+1 ];
}
// Compute Spine-aligned Cross-section Plane (SCP)
if (! spineStraight)
{
float S1[3], S2[3]; // Spine vectors [i,i-1] and [i,i+1]
int yi1, yi2, si1, s1i2, s2i2;
if (spineClosed && (i == 0 || i == nSpine-1))
{
yi1 = 3*(nSpine-2);
yi2 = 3;
si1 = 0;
s1i2 = 3*(nSpine-2);
s2i2 = 3;
}
else if (i == 0)
{
yi1 = 0;
yi2 = 3;
si1 = 3;
s1i2 = 0;
s2i2 = 6;
}
else if (i == nSpine-1)
{
yi1 = 3*(nSpine-2);
yi2 = 3*(nSpine-1);
si1 = 3*(nSpine-2);
s1i2 = 3*(nSpine-3);
s2i2 = 3*(nSpine-1);
}
else
{
yi1 = 3*(i-1);
yi2 = 3*(i+1);
si1 = 3*i;
s1i2 = 3*(i-1);
s2i2 = 3*(i+1);
}
Vdiff( Yscp, &spine[yi2], &spine[yi1] );
Vdiff( S1, &spine[s1i2], &spine[si1] );
Vdiff( S2, &spine[s2i2], &spine[si1] );
Vnorm( Yscp );
Vset(lastZ, Zscp); // Save last Zscp
Vcross( Zscp, S2, S1 );
float VlenZ = (float)Vlength(Zscp);
if ( VlenZ == 0.0 )
Vset(Zscp, lastZ);
else
Vscale( Zscp, 1.0f/VlenZ );
if ((i > 0) && (Vdot( Zscp, lastZ ) < 0.0))
Vscale( Zscp, -1.0 );
Vcross( Xscp, Yscp, Zscp );
}
// Rotate cross section into SCP
for (j = 0; j < nCrossSection; ++j) {
float cx, cy, cz;
cx = c[3*(ci+j)+0]*Xscp[0]+c[3*(ci+j)+1]*Yscp[0]+c[3*(ci+j)+2]*Zscp[0];
cy = c[3*(ci+j)+0]*Xscp[1]+c[3*(ci+j)+1]*Yscp[1]+c[3*(ci+j)+2]*Zscp[1];
cz = c[3*(ci+j)+0]*Xscp[2]+c[3*(ci+j)+1]*Yscp[2]+c[3*(ci+j)+2]*Zscp[2];
c[3*(ci+j)+0] = cx;
c[3*(ci+j)+1] = cy;
c[3*(ci+j)+2] = cz;
}
// Apply orientation
if (! FPZERO(orientation[3]) )
{
if (nOrientation > 1)
Mrotation( om, orientation );
for (j = 0; j < nCrossSection; ++j) {
float cx, cy, cz;
cx = (float)(c[3*(ci+j)+0]*om[0][0]+c[3*(ci+j)+1]*om[1][0]+c[3*(ci+j)+2]*om[2][0]);
cy = (float)(c[3*(ci+j)+0]*om[0][1]+c[3*(ci+j)+1]*om[1][1]+c[3*(ci+j)+2]*om[2][1]);
cz = (float)(c[3*(ci+j)+0]*om[0][2]+c[3*(ci+j)+1]*om[1][2]+c[3*(ci+j)+2]*om[2][2]);
c[3*(ci+j)+0] = cx;
c[3*(ci+j)+1] = cy;
c[3*(ci+j)+2] = cz;
}
}
// Translate cross section
for (j = 0; j < nCrossSection; ++j) {
c[3*(ci+j)+0] += spine[3*i+0];
c[3*(ci+j)+1] += spine[3*i+1];
c[3*(ci+j)+2] += spine[3*i+2];
// Texture coords
tc[3*(ci+j)+0] = ((float) j) / (nCrossSection-1);
tc[3*(ci+j)+1] = 1.0f - ((float) i) / (nSpine-1);
tc[3*(ci+j)+2] = 0.0f;
}
if (nScale > 1) scale += 2;
if (nOrientation > 1) orientation += 4;
}
// And compute face indices:
if (faces)
{
int polyIndex = 0;
for (i = 0, ci = 0; i < nSpine-1; ++i, ci+=nCrossSection) {
for (j = 0; j < nCrossSection-1; ++j) {
faces[polyIndex + 0] = ci+j;
faces[polyIndex + 1] = ci+j+1;
faces[polyIndex + 2] = ci+j+1 + nCrossSection;
faces[polyIndex + 3] = ci+j + nCrossSection;
faces[polyIndex + 4] = -1;
polyIndex += 5;
}
}
}
}
void HMMWV_ReissnerTire::CreateMesh(const ChFrameMoving<>& wheel_frame, VehicleSide side) {
// Create piece-wise cubic spline approximation of the tire profile.
// x - radial direction
// y - transversal direction
ChCubicSpline splineX(m_profile_t, m_profile_x);
ChCubicSpline splineY(m_profile_t, m_profile_y);
// Create the mesh nodes.
// The nodes are first created in the wheel local frame, assuming Y as the tire axis,
// and are then transformed to the global frame.
for (int i = 0; i < m_div_circumference; i++) {
double phi = (CH_C_2PI * i) / m_div_circumference;
ChVector<> nrm(-std::sin(phi), 0, std::cos(phi));
for (int j = 0; j <= m_div_width; j++) {
double t_prf = double(j) / m_div_width;
double x_prf, xp_prf, xpp_prf;
double y_prf, yp_prf, ypp_prf;
splineX.Evaluate(t_prf, x_prf, xp_prf, xpp_prf);
splineY.Evaluate(t_prf, y_prf, yp_prf, ypp_prf);
// Node position with respect to rim center
double x = (m_rim_radius + x_prf) * std::cos(phi);
double y = y_prf;
double z = (m_rim_radius + x_prf) * std::sin(phi);
// Node position in global frame (actual coordinate values)
ChVector<> loc = wheel_frame.TransformPointLocalToParent(ChVector<>(x, y, z));
// Node direction
ChVector<> tan_prf(std::cos(phi) * xp_prf, yp_prf, std::sin(phi) * xp_prf);
ChVector<> nrm_prf = Vcross(tan_prf, nrm).GetNormalized();
ChVector<> dir = wheel_frame.TransformDirectionLocalToParent(nrm_prf);
ChMatrix33<> mrot; mrot.Set_A_Xdir(tan_prf,nrm_prf);
auto node = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(loc, mrot));
// Node velocity
ChVector<> vel = wheel_frame.PointSpeedLocalToParent(ChVector<>(x, y, z));
node->SetPos_dt(vel);
node->SetMass(0);
m_mesh->AddNode(node);
}
}
// Create the Reissner shell elements
for (int i = 0; i < m_div_circumference; i++) {
for (int j = 0; j < m_div_width; j++) {
// Adjacent nodes
int inode0, inode1, inode2, inode3;
inode1 = j + i * (m_div_width + 1);
inode2 = j + 1 + i * (m_div_width + 1);
if (i == m_div_circumference - 1) {
inode0 = j;
inode3 = j + 1;
} else {
inode0 = j + (i + 1) * (m_div_width + 1);
inode3 = j + 1 + (i + 1) * (m_div_width + 1);
}
auto node0 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode0));
auto node1 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode1));
auto node2 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode2));
auto node3 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode3));
// Create the element and set its nodes.
auto element = std::make_shared<ChElementShellReissner4>();
element->SetNodes(node0, node1, node2, node3);
// Element dimensions
double len_circumference =
0.5 * ((node1->GetPos() - node0->GetPos()).Length() + (node3->GetPos() - node2->GetPos()).Length());
double len_width = (node2->GetPos() - node0->GetPos()).Length();
// Figure out the section for this element
int b1 = m_num_elements_bead;
int b2 = m_div_width - m_num_elements_bead;
int s1 = b1 + m_num_elements_sidewall;
int s2 = b2 - m_num_elements_sidewall;
if (j < b1 || j >= b2) {
// Bead section
for (unsigned int im = 0; im < m_num_layers_bead; im++) {
element->AddLayer(m_layer_thickness_bead[im], CH_C_DEG_TO_RAD * m_ply_angle_bead[im],
m_materials[m_material_id_bead[im]]);
}
} else if (j < s1 || j >= s2) {
// Sidewall section
for (unsigned int im = 0; im < m_num_layers_sidewall; im++) {
element->AddLayer(m_layer_thickness_sidewall[im], CH_C_DEG_TO_RAD * m_ply_angle_sidewall[im],
m_materials[m_material_id_sidewall[im]]);
}
} else {
// Tread section
for (unsigned int im = 0; im < m_num_layers_tread; im++) {
element->AddLayer(m_layer_thickness_tread[im], CH_C_DEG_TO_RAD * m_ply_angle_tread[im],
m_materials[m_material_id_tread[im]]);
}
}
// Set other element properties
element->SetAlphaDamp(m_alpha);
// Add element to mesh
m_mesh->AddElement(element);
}
}
// Switch on automatic gravity
m_mesh->SetAutomaticGravity(true);
}
double ChPathSteeringControllerXT::Advance(const ChVehicle& vehicle, double step) {
// Calculate current "sentinel" location. This is a point at the look-ahead
// distance in front of the vehicle.
m_sentinel = vehicle.GetChassisBody()->GetFrame_REF_to_abs().TransformPointLocalToParent(ChVector<>(m_dist, 0, 0));
m_vel = vehicle.GetVehiclePointVelocity(ChVector<>(0,0,0));
if(!m_filters_initialized) {
// first time we know about step size
m_HeadErrDelay.Config(step, m_T1_delay);
m_AckermannAngleDelay.Config(step, m_T1_delay);
m_PathErrCtl.Config(step,0.3,0.15,m_Kp);
m_filters_initialized = true;
}
// Calculate current "target" location.
CalcTargetLocation();
// If data collection is enabled, append current target and sentinel locations.
if (m_collect) {
*m_csv << vehicle.GetChTime() << m_target << m_sentinel << std::endl;
}
// The "error" vector is the projection onto the horizontal plane (z=0) of
// the vector between sentinel and target.
ChVector<> err_vec = m_target - m_sentinel;
err_vec.z() = 0;
// Calculate the sign of the angle between the projections of the sentinel
// vector and the target vector (with origin at vehicle location).
ChVector<> sentinel_vec = m_sentinel - vehicle.GetVehiclePos();
sentinel_vec.z() = 0;
ChVector<> target_vec = m_target - vehicle.GetVehiclePos();
target_vec.z() = 0;
double temp = Vdot(Vcross(sentinel_vec, target_vec), ChVector<>(0, 0, 1));
// Calculate current lateral error.
double y_err = ChSignum(temp) * err_vec.Length();
double y_err_out = m_PathErrCtl.Filter(y_err);
// Calculate the heading error
ChVector<> veh_head = vehicle.GetVehicleRot().GetXaxis();
ChVector<> path_head = m_ptangent;
double h_err = CalcHeadingError(veh_head,path_head);
double h_err_out = m_HeadErrDelay.Filter(h_err);
// Calculate the Ackermann angle
double a_err = CalcAckermannAngle();
double a_err_out = m_AckermannAngleDelay.Filter(a_err);
// Calculate the resultant steering signal
double res = m_Wy * y_err_out + m_Wh * h_err_out + m_Wa * a_err_out;
// Additional processing is necessary: counter steer constraint
// in left bending curves only left steering allowed
// in right bending curves only right steering allowed
// |res| is never allowed to grow above 1
ChVector<> veh_left = vehicle.GetVehicleRot().GetYaxis();
ChVector<> path_left = m_pnormal;
int crvcode = CalcCurvatureCode(veh_left,path_left);
switch(crvcode) {
case 1:
m_res = ChClamp<>(res,0.0,1.0);
break;
case -1:
m_res = ChClamp<>(res,-1.0,0.0);
break;
default:
case 0:
m_res = ChClamp<>(res,-1.0,1.0);
break;
}
return m_res;
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChFialaTire::Advance(double step) {
if (m_data.in_contact) {
// Take as many integration steps as needed to reach the value 'step'
double t = 0;
while (t < step) {
// Ensure we integrate exactly to 'step'
double h = std::min<>(m_stepsize, step - t);
// Advance state for longitudinal direction
// integrate using trapezoidal rule integration since this equation is linear
// cp_long_slip_dot = -1/m_relax_length_x*(Vsx+(abs(Vx)*cp_long_slip))
m_states.cp_long_slip =
((2 * m_relax_length_x - h * m_states.abs_vx) * m_states.cp_long_slip - 2 * h * m_states.vsx) /
(2 * m_relax_length_x + h * m_states.abs_vx);
#if(fialaUseSmallAngle == 0)
// integrate using RK2 since this equation is non-linear
// cp_side_slip = 1/m_relax_length_y*(Vsy-(abs(Vx)*tan(cp_long_slip)))
double k1 = h / m_relax_length_y*(m_states.vsy - m_states.abs_vx*std::tan(m_states.cp_side_slip));
double temp = std::max<>(-CH_C_PI_2 + .0001, std::min<>(CH_C_PI_2 - .0001, m_states.cp_side_slip+k1/2));
double k2 = h / m_relax_length_y*(m_states.vsy - m_states.abs_vx*std::tan(temp));
m_states.cp_side_slip = m_states.cp_side_slip + k2;
#else
// Advance state for lateral direction
// integrate using trapezoidal rule integration since this equation is linear
// after using a small angle approximation for tan(alpha)
// cp_long_slip_dot = -1/m_relax_length_x*(Vsx+(abs(Vx)*cp_long_slip))
m_states.cp_side_slip =
((2 * m_relax_length_y - h * m_states.abs_vx) * m_states.cp_side_slip + 2 * h * m_states.vsy) /
(2 * m_relax_length_y + h * m_states.abs_vx);
#endif
// Ensure that cp_lon_slip stays between -1 & 1
m_states.cp_long_slip = std::max<>(-1., std::min<>(1., m_states.cp_long_slip));
// Ensure that cp_side_slip stays between -pi()/2 & pi()/2 (a little less to prevent tan from going to infinity)
m_states.cp_side_slip = std::max<>(-CH_C_PI_2+.0001, std::min<>(CH_C_PI_2-.0001, m_states.cp_side_slip));
t += h;
}
////Overwrite with steady-state alpha & kappa for debugging
// if (m_states.abs_vx != 0) {
// m_states.cp_long_slip = -m_states.vsx / m_states.abs_vx;
// m_states.cp_side_slip = std::atan2(m_states.vsy , m_states.abs_vx);
//}
// else {
// m_states.cp_long_slip = 0;
// m_states.cp_side_slip = 0;
//}
// Now calculate the new force and moment values (normal force and moment has already been accounted for in
// Synchronize())
// See reference for more detail on the calculations
double SsA = std::min<>(1.0,std::sqrt(std::pow(m_states.cp_long_slip, 2) + std::pow(std::tan(m_states.cp_side_slip), 2)));
double U = m_u_max - (m_u_max - m_u_min) * SsA;
double S_critical = std::abs(U * m_data.normal_force / (2 * m_c_slip));
double Alpha_critical = std::atan(3 * U * m_data.normal_force / m_c_alpha);
double Fx;
double Fy;
double My;
double Mz;
// Longitudinal Force:
if (std::abs(m_states.cp_long_slip) < S_critical) {
Fx = m_c_slip * m_states.cp_long_slip;
} else {
double Fx1 = U * m_data.normal_force;
double Fx2 =
std::abs(std::pow((U * m_data.normal_force), 2) / (4 * m_states.cp_long_slip * m_c_slip));
Fx = sgn(m_states.cp_long_slip) * (Fx1 - Fx2);
}
// Lateral Force & Aligning Moment (Mz):
if (std::abs(m_states.cp_side_slip) <= Alpha_critical) {
double H = 1 - m_c_alpha * std::abs(std::tan(m_states.cp_side_slip)) / (3 * U * m_data.normal_force);
Fy = -U * m_data.normal_force * (1 - std::pow(H, 3)) * sgn(m_states.cp_side_slip);
Mz = U * m_data.normal_force * m_width * (1 - H) * std::pow(H, 3) * sgn(m_states.cp_side_slip);
} else {
Fy = -U * m_data.normal_force * sgn(m_states.cp_side_slip);
Mz = 0;
}
// Rolling Resistance
My = -m_rolling_resistance * m_data.normal_force * sgn(m_states.omega);
// compile the force and moment vectors so that they can be
// transformed into the global coordinate system
m_tireforce.force = ChVector<>(Fx, Fy, m_data.normal_force);
m_tireforce.moment = ChVector<>(0, My, Mz);
// Rotate into global coordinates
m_tireforce.force = m_data.frame.TransformDirectionLocalToParent(m_tireforce.force);
m_tireforce.moment = m_data.frame.TransformDirectionLocalToParent(m_tireforce.moment);
// Move the tire forces from the contact patch to the wheel center
m_tireforce.moment +=
Vcross((m_data.frame.pos + m_data.depth*m_data.frame.rot.GetZaxis()) - m_tireforce.point, m_tireforce.force);
}
// Else do nothing since the "m_tireForce" force and moment values are already 0 (set in Synchronize())
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPac89Tire::Advance(double step) {
// Return now if no contact. Tire force and moment are already set to 0 in Synchronize().
if (!m_data.in_contact)
return;
if (m_states.vx != 0) {
m_states.cp_long_slip = -m_states.vsx / m_states.vx;
} else {
m_states.cp_long_slip = 0;
}
if (m_states.omega != 0) {
m_states.cp_side_slip = std::atan(m_states.vsy / std::abs(m_states.omega * (m_unloaded_radius - m_data.depth)));
} else {
m_states.cp_side_slip = 0;
}
// Ensure that cp_lon_slip stays between -1 & 1
ChClampValue(m_states.cp_long_slip, -1.0, 1.0);
// Ensure that cp_side_slip stays between -pi()/2 & pi()/2 (a little less to prevent tan from going to infinity)
ChClampValue(m_states.cp_side_slip, -CH_C_PI_2 + 0.001, CH_C_PI_2 - 0.001);
// Calculate the new force and moment values (normal force and moment have already been accounted for in
// Synchronize()).
// Express Fz in kN (note that all other forces and moments are in N and Nm).
// See reference for details on the calculations.
double Fx = 0;
double Fy = 0;
double Fz = m_data.normal_force / 1000;
double Mx = 0;
double My = 0;
double Mz = 0;
// Express alpha and gamma in degrees. Express kappa as percentage.
// Flip sign of alpha to convert to PAC89 modified SAE coordinates.
m_gamma = 90.0 - std::acos(m_states.disc_normal.z()) * CH_C_RAD_TO_DEG;
m_alpha = -m_states.cp_side_slip * CH_C_RAD_TO_DEG;
m_kappa = m_states.cp_long_slip * 100.0;
// Clamp |gamma| to specified value: Limit due to tire testing, avoids erratic extrapolation.
double gamma = ChClamp(m_gamma, -m_gamma_limit, m_gamma_limit);
// Longitudinal Force
{
double C = m_PacCoeff.B0;
double D = (m_PacCoeff.B1 * std::pow(Fz, 2) + m_PacCoeff.B2 * Fz);
double BCD = (m_PacCoeff.B3 * std::pow(Fz, 2) + m_PacCoeff.B4 * Fz) * std::exp(-m_PacCoeff.B5 * Fz);
double B = BCD / (C * D);
double Sh = m_PacCoeff.B9 * Fz + m_PacCoeff.B10;
double Sv = 0.0;
double X1 = (m_kappa + Sh);
double E = (m_PacCoeff.B6 * std::pow(Fz, 2) + m_PacCoeff.B7 * Fz + m_PacCoeff.B8);
Fx = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv;
}
// Lateral Force
{
double C = m_PacCoeff.A0;
double D = (m_PacCoeff.A1 * std::pow(Fz, 2) + m_PacCoeff.A2 * Fz);
double BCD =
m_PacCoeff.A3 * std::sin(std::atan(Fz / m_PacCoeff.A4) * 2.0) * (1.0 - m_PacCoeff.A5 * std::abs(gamma));
double B = BCD / (C * D);
double Sh = m_PacCoeff.A9 * Fz + m_PacCoeff.A10 + m_PacCoeff.A8 * gamma;
double Sv = m_PacCoeff.A11 * Fz * gamma + m_PacCoeff.A12 * Fz + m_PacCoeff.A13;
double X1 = m_alpha + Sh;
double E = m_PacCoeff.A6 * Fz + m_PacCoeff.A7;
// Ensure that X1 stays within +/-90 deg minus a little bit
ChClampValue(X1, -89.5, 89.5);
Fy = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv;
}
// Self-Aligning Torque
{
double C = m_PacCoeff.C0;
double D = (m_PacCoeff.C1 * std::pow(Fz, 2) + m_PacCoeff.C2 * Fz);
double BCD = (m_PacCoeff.C3 * std::pow(Fz, 2) + m_PacCoeff.C4 * Fz) * (1 - m_PacCoeff.C6 * std::abs(gamma)) *
std::exp(-m_PacCoeff.C5 * Fz);
double B = BCD / (C * D);
double Sh = m_PacCoeff.C11 * gamma + m_PacCoeff.C12 * Fz + m_PacCoeff.C13;
double Sv =
(m_PacCoeff.C14 * std::pow(Fz, 2) + m_PacCoeff.C15 * Fz) * gamma + m_PacCoeff.C16 * Fz + m_PacCoeff.C17;
double X1 = m_alpha + Sh;
double E = (m_PacCoeff.C7 * std::pow(Fz, 2) + m_PacCoeff.C8 * Fz + m_PacCoeff.C9) *
(1.0 - m_PacCoeff.C10 * std::abs(gamma));
// Ensure that X1 stays within +/-90 deg minus a little bit
ChClampValue(X1, -89.5, 89.5);
Mz = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv;
}
// Overturning Moment
{
double deflection = Fy / m_lateral_stiffness;
Mx = -(Fz * 1000) * deflection;
Mz = Mz + Fx * deflection;
}
// Rolling Resistance
{
double Lrad = (m_unloaded_radius - m_data.depth);
// Smoothing interval for My
const double vx_min = 0.125;
const double vx_max = 0.5;
// Smoothing factor dependend on m_state.abs_vx, allows soft switching of My
double myStartUp = ChSineStep(std::abs(m_states.vx), vx_min, 0.0, vx_max, 1.0);
My = myStartUp * m_rolling_resistance * m_data.normal_force * Lrad * ChSignum(m_states.omega);
}
// std::cout << "Fx:" << Fx
// << " Fy:" << Fy
// << " Fz:" << Fz
// << " Mx:" << Mx
// << " My:" << My
// << " Mz:" << Mz
// << std::endl
// << " G:" << gamma
// << " A:" << alpha
// << " K:" << kappa
// << " O:" << m_states.omega
// << std::endl;
// Compile the force and moment vectors so that they can be
// transformed into the global coordinate system.
// Convert from SAE to ISO Coordinates at the contact patch.
m_tireforce.force = ChVector<>(Fx, -Fy, m_data.normal_force);
m_tireforce.moment = ChVector<>(Mx, -My, -Mz);
// Rotate into global coordinates
m_tireforce.force = m_data.frame.TransformDirectionLocalToParent(m_tireforce.force);
m_tireforce.moment = m_data.frame.TransformDirectionLocalToParent(m_tireforce.moment);
// Move the tire forces from the contact patch to the wheel center
m_tireforce.moment +=
Vcross((m_data.frame.pos + m_data.depth * m_data.frame.rot.GetZaxis()) - m_tireforce.point, m_tireforce.force);
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
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);
}
