// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChChassis::Initialize(ChSystem* system,
const ChCoordsys<>& chassisPos,
double chassisFwdVel,
int collision_family) {
m_body = std::shared_ptr<ChBodyAuxRef>(system->NewBodyAuxRef());
m_body->SetIdentifier(0);
m_body->SetNameString(m_name + "_body");
m_body->SetMass(GetMass());
m_body->SetFrame_COG_to_REF(ChFrame<>(GetLocalPosCOM(), ChQuaternion<>(1, 0, 0, 0)));
m_body->SetInertia(GetInertia());
m_body->SetBodyFixed(m_fixed);
m_body->SetFrame_REF_to_abs(ChFrame<>(chassisPos));
m_body->SetPos_dt(chassisFwdVel * chassisPos.TransformDirectionLocalToParent(ChVector<>(1, 0, 0)));
system->Add(m_body);
}
// =============================================================================
//
// Worker function for performing the simulation with specified parameters.
//
bool TestLinActuator(const ChQuaternion<>& rot, // translation along Z axis
double desiredSpeed, // imposed translation speed
double simTimeStep, // simulation time step
double outTimeStep, // output time step
const std::string& testName, // name of this test
bool animate) // if true, animate with Irrlich
{
std::cout << "TEST: " << testName << std::endl;
// Unit vector along translation axis, expressed in global frame
ChVector<> axis = rot.GetZaxis();
// Settings
//---------
double mass = 1.0; // mass of plate
ChVector<> inertiaXX(1, 1, 1); // mass moments of inertia of plate (centroidal frame)
double g = 9.80665;
double timeRecord = 5; // Stop recording to the file after this much simulated time
// Create the mechanical system
// ----------------------------
ChSystem my_system;
my_system.Set_G_acc(ChVector<>(0.0, 0.0, -g));
my_system.SetIntegrationType(ChSystem::INT_ANITESCU);
my_system.SetIterLCPmaxItersSpeed(100);
my_system.SetIterLCPmaxItersStab(100); //Tasora stepper uses this, Anitescu does not
my_system.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
my_system.SetTol(1e-6);
my_system.SetTolForce(1e-4);
// Create the ground body.
ChSharedBodyPtr ground(new ChBody);
my_system.AddBody(ground);
ground->SetBodyFixed(true);
// Add geometry to the ground body for visualizing the translational joint
ChSharedPtr<ChBoxShape> box_g(new ChBoxShape);
box_g->GetBoxGeometry().SetLengths(ChVector<>(0.1, 0.1, 5));
box_g->GetBoxGeometry().Pos = 2.5 * axis;
box_g->GetBoxGeometry().Rot = rot;
ground->AddAsset(box_g);
ChSharedPtr<ChColorAsset> col_g(new ChColorAsset);
col_g->SetColor(ChColor(0.6f, 0.2f, 0.2f));
ground->AddAsset(col_g);
// Create the plate body.
ChSharedBodyPtr plate(new ChBody);
my_system.AddBody(plate);
plate->SetPos(ChVector<>(0, 0, 0));
plate->SetRot(rot);
plate->SetPos_dt(desiredSpeed * axis);
plate->SetMass(mass);
plate->SetInertiaXX(inertiaXX);
// Add geometry to the plate for visualization
ChSharedPtr<ChBoxShape> box_p(new ChBoxShape);
box_p->GetBoxGeometry().SetLengths(ChVector<>(1, 1, 0.2));
plate->AddAsset(box_p);
ChSharedPtr<ChColorAsset> col_p(new ChColorAsset);
col_p->SetColor(ChColor(0.2f, 0.2f, 0.6f));
plate->AddAsset(col_p);
// Create prismatic (translational) joint between plate and ground.
// We set the ground as the "master" body (second one in the initialization
// call) so that the link coordinate system is expressed in the ground frame.
ChSharedPtr<ChLinkLockPrismatic> prismatic(new ChLinkLockPrismatic);
prismatic->Initialize(plate, ground, ChCoordsys<>(ChVector<>(0, 0, 0), rot));
my_system.AddLink(prismatic);
// Create a ramp function to impose constant speed. This function returns
// y(t) = 0 + t * desiredSpeed
// y'(t) = desiredSpeed
ChSharedPtr<ChFunction_Ramp> actuator_fun(new ChFunction_Ramp(0.0, desiredSpeed));
// Create the linear actuator, connecting the plate to the ground.
// Here, we set the plate as the master body (second one in the initialization
// call) so that the link coordinate system is expressed in the plate body
// frame.
ChSharedPtr<ChLinkLinActuator> actuator(new ChLinkLinActuator);
ChVector<> pt1 = ChVector<>(0, 0, 0);
ChVector<> pt2 = axis;
actuator->Initialize(ground, plate, false, ChCoordsys<>(pt1, rot), ChCoordsys<>(pt2, rot));
actuator->Set_lin_offset(1);
actuator->Set_dist_funct(actuator_fun);
my_system.AddLink(actuator);
// Perform the simulation (animation with Irrlicht)
// ------------------------------------------------
if (animate)
{
// Create the Irrlicht application for visualization
ChIrrApp application(&my_system, L"ChLinkRevolute demo", core::dimension2d<u32>(800, 600), false, true);
application.AddTypicalLogo();
application.AddTypicalSky();
application.AddTypicalLights();
application.AddTypicalCamera(core::vector3df(1, 2, -2), core::vector3df(0, 0, 2));
application.AssetBindAll();
application.AssetUpdateAll();
application.SetStepManage(true);
application.SetTimestep(simTimeStep);
// Simulation loop
while (application.GetDevice()->run())
{
application.BeginScene();
application.DrawAll();
// Draw an XZ grid at the global origin to add in visualization
ChIrrTools::drawGrid(
application.GetVideoDriver(), 1, 1, 20, 20,
ChCoordsys<>(ChVector<>(0, 0, 0), Q_from_AngX(CH_C_PI_2)),
video::SColor(255, 80, 100, 100), true);
application.DoStep(); //Take one step in time
application.EndScene();
}
return true;
}
// Perform the simulation (record results)
// ------------------------------------------------
// Create the CSV_Writer output objects (TAB delimited)
utils::CSV_writer out_pos = OutStream();
utils::CSV_writer out_vel = OutStream();
utils::CSV_writer out_acc = OutStream();
utils::CSV_writer out_quat = OutStream();
utils::CSV_writer out_avel = OutStream();
utils::CSV_writer out_aacc = OutStream();
utils::CSV_writer out_rfrcP = OutStream();
utils::CSV_writer out_rtrqP = OutStream();
utils::CSV_writer out_rfrcA = OutStream();
utils::CSV_writer out_rtrqA = OutStream();
utils::CSV_writer out_cnstrP = OutStream();
utils::CSV_writer out_cnstrA = OutStream();
// Write headers
out_pos << "Time" << "X_Pos" << "Y_Pos" << "Z_Pos" << std::endl;
out_vel << "Time" << "X_Vel" << "Y_Vel" << "Z_Vel" << std::endl;
out_acc << "Time" << "X_Acc" << "Y_Acc" << "Z_Acc" << std::endl;
out_quat << "Time" << "e0" << "e1" << "e2" << "e3" << std::endl;
out_avel << "Time" << "X_AngVel" << "Y_AngVel" << "Z_AngVel" << std::endl;
out_aacc << "Time" << "X_AngAcc" << "Y_AngAcc" << "Z_AngAcc" << std::endl;
out_rfrcP << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
out_rtrqP << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;
out_rfrcA << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
out_rtrqA << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;
out_cnstrP << "Time" << "Cnstr_1" << "Cnstr_2" << "Cnstr_3" << "Constraint_4" << "Cnstr_5" << std::endl;
out_cnstrA << "Time" << "Cnstr_1" << std::endl;
// Simulation loop
double simTime = 0;
double outTime = 0;
while (simTime <= timeRecord + simTimeStep / 2)
{
// Ensure that the final data point is recorded.
if (simTime >= outTime - simTimeStep / 2)
{
// CM position, velocity, and acceleration (expressed in global frame).
const ChVector<>& position = plate->GetPos();
const ChVector<>& velocity = plate->GetPos_dt();
out_pos << simTime << position << std::endl;
out_vel << simTime << velocity << std::endl;
out_acc << simTime << plate->GetPos_dtdt() << std::endl;
// Orientation, angular velocity, and angular acceleration (expressed in
// global frame).
out_quat << simTime << plate->GetRot() << std::endl;
out_avel << simTime << plate->GetWvel_par() << std::endl;
out_aacc << simTime << plate->GetWacc_par() << std::endl;
// Reaction Force and Torque in prismatic joint.
// These are expressed in the link coordinate system. We convert them to
// the coordinate system of Body2 (in our case this is the ground).
ChCoordsys<> linkCoordsysP = prismatic->GetLinkRelativeCoords();
ChVector<> reactForceP = prismatic->Get_react_force();
ChVector<> reactForceGlobalP = linkCoordsysP.TransformDirectionLocalToParent(reactForceP);
out_rfrcP << simTime << reactForceGlobalP << std::endl;
ChVector<> reactTorqueP = prismatic->Get_react_torque();
ChVector<> reactTorqueGlobalP = linkCoordsysP.TransformDirectionLocalToParent(reactTorqueP);
out_rtrqP << simTime << reactTorqueGlobalP << std::endl;
// Reaction force and Torque in linear actuator.
// These are expressed in the link coordinate system. We convert them to
// the coordinate system of Body2 (in our case this is the plate). As such,
// the reaction force represents the force that needs to be applied to the
// plate in order to maintain the prescribed constant velocity. These are
// then converted to the global frame for comparison to ADAMS
ChCoordsys<> linkCoordsysA = actuator->GetLinkRelativeCoords();
ChVector<> reactForceA = actuator->Get_react_force();
reactForceA = linkCoordsysA.TransformDirectionLocalToParent(reactForceA);
ChVector<> reactForceGlobalA = plate->TransformDirectionLocalToParent(reactForceA);
out_rfrcA << simTime << reactForceGlobalA << std::endl;
ChVector<> reactTorqueA = actuator->Get_react_torque();
reactTorqueA = linkCoordsysA.TransformDirectionLocalToParent(reactTorqueA);
ChVector<> reactTorqueGlobalA = plate->TransformDirectionLocalToParent(reactTorqueA);
out_rtrqA << simTime << reactTorqueGlobalA << std::endl;
// Constraint violations in prismatic joint
ChMatrix<>* CP = prismatic->GetC();
out_cnstrP << simTime
<< CP->GetElement(0, 0)
<< CP->GetElement(1, 0)
<< CP->GetElement(2, 0)
<< CP->GetElement(3, 0)
<< CP->GetElement(4, 0) << std::endl;
// Constraint violations in linear actuator
ChMatrix<>* CA = actuator->GetC();
out_cnstrA << simTime << CA->GetElement(0, 0) << std::endl;
// Increment output time
outTime += outTimeStep;
}
// Advance simulation by one step
my_system.DoStepDynamics(simTimeStep);
// Increment simulation time
simTime += simTimeStep;
}
// Write output files
out_pos.write_to_file(out_dir + testName + "_CHRONO_Pos.txt", testName + "\n\n");
out_vel.write_to_file(out_dir + testName + "_CHRONO_Vel.txt", testName + "\n\n");
out_acc.write_to_file(out_dir + testName + "_CHRONO_Acc.txt", testName + "\n\n");
out_quat.write_to_file(out_dir + testName + "_CHRONO_Quat.txt", testName + "\n\n");
out_avel.write_to_file(out_dir + testName + "_CHRONO_Avel.txt", testName + "\n\n");
out_aacc.write_to_file(out_dir + testName + "_CHRONO_Aacc.txt", testName + "\n\n");
out_rfrcP.write_to_file(out_dir + testName + "_CHRONO_RforceP.txt", testName + "\n\n");
out_rtrqP.write_to_file(out_dir + testName + "_CHRONO_RtorqueP.txt", testName + "\n\n");
out_rfrcA.write_to_file(out_dir + testName + "_CHRONO_RforceA.txt", testName + "\n\n");
out_rtrqA.write_to_file(out_dir + testName + "_CHRONO_RtorqueA.txt", testName + "\n\n");
out_cnstrP.write_to_file(out_dir + testName + "_CHRONO_ConstraintsP.txt", testName + "\n\n");
out_cnstrA.write_to_file(out_dir + testName + "_CHRONO_ConstraintsA.txt", testName + "\n\n");
return true;
}
// =============================================================================
//
// Worker function for performing the simulation with specified parameters.
//
bool TestDistance(const ChVector<>& jointLocGnd, // absolute location of the distance constrain ground attachment point
const ChVector<>& jointLocPend, // absolute location of the distance constrain pendulum attachment point
const ChCoordsys<>& PendCSYS, // Coordinate system for the pendulum
double simTimeStep, // simulation time step
double outTimeStep, // output time step
const std::string& testName, // name of this test
bool animate, // if true, animate with Irrlich
bool save) // if true, also save animation data
{
std::cout << "TEST: " << testName << std::endl;
// Settings
//---------
// There are no units in Chrono, so values must be consistent
// (MKS is used in this example)
double mass = 1.0; // mass of pendulum
double length = 4.0; // length of pendulum
ChVector<> inertiaXX(0.04, 0.1, 0.1); // mass moments of inertia of pendulum (centroidal frame)
double g = 9.80665;
double timeRecord = 5; // simulation length
// Create the mechanical system
// ----------------------------
// Create a ChronoENGINE physical system: all bodies and constraints will be
// handled by this ChSystem object.
ChSystem my_system;
my_system.Set_G_acc(ChVector<>(0.0, 0.0, -g));
my_system.SetIntegrationType(ChSystem::INT_ANITESCU);
my_system.SetIterLCPmaxItersSpeed(100);
my_system.SetIterLCPmaxItersStab(100); //Tasora stepper uses this, Anitescu does not
my_system.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
my_system.SetTol(1e-6);
my_system.SetTolForce(1e-4);
// Create the ground body
ChSharedBodyPtr ground(new ChBody);
my_system.AddBody(ground);
ground->SetBodyFixed(true);
// Add some geometry to the ground body for visualizing the revolute joint
ChSharedPtr<ChSphereShape> sph_g(new ChSphereShape);
sph_g->GetSphereGeometry().center = jointLocGnd;
sph_g->GetSphereGeometry().rad = 0.05;
ground->AddAsset(sph_g);
// Create the pendulum body in an initial configuration at rest, with an
// orientatoin that matches the specified joint orientation and a position
// consistent with the specified joint location.
// The pendulum CG is assumed to be at half its length.
ChSharedBodyPtr pendulum(new ChBody);
my_system.AddBody(pendulum);
pendulum->SetPos(PendCSYS.pos);
pendulum->SetRot(PendCSYS.rot);
pendulum->SetMass(mass);
pendulum->SetInertiaXX(inertiaXX);
// Add some geometry to the pendulum for visualization
ChSharedPtr<ChSphereShape> sph_p(new ChSphereShape);
sph_p->GetSphereGeometry().center = pendulum->TransformPointParentToLocal(jointLocPend);
sph_p->GetSphereGeometry().rad = 0.05;
pendulum->AddAsset(sph_p);
ChSharedPtr<ChBoxShape> box_p(new ChBoxShape);
box_p->GetBoxGeometry().Size = ChVector<>(0.5 * length - 0.05, 0.05 * length, 0.05 * length);
pendulum->AddAsset(box_p);
// Create a distance constraint between pendulum at "jointLocPend"
// and ground at "jointLocGnd" in the global reference frame.
// The constrained distance is set equal to the inital distance between
// "jointLocPend" and "jointLocGnd".
ChSharedPtr<ChLinkDistance> distanceConstraint(new ChLinkDistance);
distanceConstraint->Initialize(pendulum, ground, false, jointLocPend, jointLocGnd, true);
my_system.AddLink(distanceConstraint);
// Perform the simulation (animation with Irrlicht option)
// -------------------------------------------------------
if (animate)
{
// Create the Irrlicht application for visualization
ChIrrApp * application = new ChIrrApp(&my_system, L"ChLinkDistance demo", core::dimension2d<u32>(800, 600), false, true);
application->AddTypicalLogo();
application->AddTypicalSky();
application->AddTypicalLights();
core::vector3df lookat((f32)jointLocGnd.x, (f32)jointLocGnd.y, (f32)jointLocGnd.z);
application->AddTypicalCamera(lookat + core::vector3df(0, 3, -6), lookat);
// Now have the visulization tool (Irrlicht) create its geometry from the
// assets defined above
application->AssetBindAll();
application->AssetUpdateAll();
application->SetTimestep(simTimeStep);
// Simulation loop
double outTime = 0;
int outFrame = 1;
std::string pov_dir = out_dir + "POVRAY_" + testName;
if (ChFileutils::MakeDirectory(pov_dir.c_str()) < 0) {
std::cout << "Error creating directory " << pov_dir << std::endl;
return false;
}
while (application->GetDevice()->run())
{
if (save && my_system.GetChTime() >= outTime - simTimeStep / 2) {
char filename[100];
sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), outFrame);
utils::WriteShapesPovray(&my_system, filename);
outTime += outTimeStep;
outFrame++;
}
application->BeginScene();
application->DrawAll();
// Render the distance constraint
ChIrrTools::drawSegment(
application->GetVideoDriver(),
distanceConstraint->GetEndPoint1Abs(),
distanceConstraint->GetEndPoint2Abs(),
video::SColor(255, 200, 20, 0), true);
// Draw an XZ grid at the global origin to add in visualization
ChIrrTools::drawGrid(
application->GetVideoDriver(), 1, 1, 20, 20,
ChCoordsys<>(ChVector<>(0, 0, 0), Q_from_AngX(CH_C_PI_2)),
video::SColor(255, 80, 100, 100), true);
application->DoStep(); //Take one step in time
application->EndScene();
}
return true;
}
// Perform the simulation (record results option)
// ------------------------------------------------
// Create the CSV_Writer output objects (TAB delimited)
utils::CSV_writer out_pos = OutStream();
utils::CSV_writer out_vel = OutStream();
utils::CSV_writer out_acc = OutStream();
utils::CSV_writer out_quat = OutStream();
utils::CSV_writer out_avel = OutStream();
utils::CSV_writer out_aacc = OutStream();
utils::CSV_writer out_rfrc = OutStream();
utils::CSV_writer out_rtrq = OutStream();
utils::CSV_writer out_energy = OutStream();
utils::CSV_writer out_cnstr = OutStream();
// Write headers
out_pos << "Time" << "X_Pos" << "Y_Pos" << "Z_Pos" << std::endl;
out_vel << "Time" << "X_Vel" << "Y_Vel" << "Z_Vel" << std::endl;
out_acc << "Time" << "X_Acc" << "Y_Acc" << "Z_Acc" << std::endl;
out_quat << "Time" << "e0" << "e1" << "e2" << "e3" << std::endl;
out_avel << "Time" << "X_AngVel" << "Y_AngVel" << "Z_AngVel" << std::endl;
out_aacc << "Time" << "X_AngAcc" << "Y_AngAcc" << "Z_AngAcc" << std::endl;
out_rfrc << "Time" << "X_Force" << "Y_Force" << "Z_Force" << std::endl;
out_rtrq << "Time" << "X_Torque" << "Y_Torque" << "Z_Torque" << std::endl;
out_energy << "Time" << "Transl_KE" << "Rot_KE" << "Delta_PE" << "KE+PE" << std::endl;
out_cnstr << "Time" << "Cnstr_1" << "Cnstr_2" << "Cnstr_3" << "Constraint_4" << "Cnstr_5" << std::endl;
// Perform a system assembly to ensure we have the correct accelerations at
// the initial time.
my_system.DoFullAssembly();
// Total energy at initial time.
ChMatrix33<> inertia = pendulum->GetInertia();
ChVector<> angVelLoc = pendulum->GetWvel_loc();
double transKE = 0.5 * mass * pendulum->GetPos_dt().Length2();
double rotKE = 0.5 * Vdot(angVelLoc, inertia * angVelLoc);
double deltaPE = mass * g * (pendulum->GetPos().z - PendCSYS.pos.z);
double totalE0 = transKE + rotKE + deltaPE;
// Simulation loop
double simTime = 0;
double outTime = 0;
while (simTime <= timeRecord + simTimeStep / 2)
{
// Ensure that the final data point is recorded.
if (simTime >= outTime - simTimeStep / 2)
{
// CM position, velocity, and acceleration (expressed in global frame).
const ChVector<>& position = pendulum->GetPos();
const ChVector<>& velocity = pendulum->GetPos_dt();
out_pos << simTime << position << std::endl;
out_vel << simTime << velocity << std::endl;
out_acc << simTime << pendulum->GetPos_dtdt() << std::endl;
// Orientation, angular velocity, and angular acceleration (expressed in
// global frame).
out_quat << simTime << pendulum->GetRot() << std::endl;
out_avel << simTime << pendulum->GetWvel_par() << std::endl;
out_aacc << simTime << pendulum->GetWacc_par() << std::endl;
// Reaction Force and Torque
// These are expressed in the link coordinate system. We convert them to
// the coordinate system of Body2 (in our case this is the ground).
ChCoordsys<> linkCoordsys = distanceConstraint->GetLinkRelativeCoords();
ChVector<> reactForce = distanceConstraint->Get_react_force();
ChVector<> reactForceGlobal = linkCoordsys.TransformDirectionLocalToParent(reactForce);
out_rfrc << simTime << reactForceGlobal << std::endl;
ChVector<> reactTorque = distanceConstraint->Get_react_torque();
ChVector<> reactTorqueGlobal = linkCoordsys.TransformDirectionLocalToParent(reactTorque);
out_rtrq << simTime << reactTorqueGlobal << std::endl;
// Conservation of Energy
// Translational Kinetic Energy (1/2*m*||v||^2)
// Rotational Kinetic Energy (1/2 w'*I*w)
// Delta Potential Energy (m*g*dz)
ChMatrix33<> inertia = pendulum->GetInertia();
ChVector<> angVelLoc = pendulum->GetWvel_loc();
double transKE = 0.5 * mass * velocity.Length2();
double rotKE = 0.5 * Vdot(angVelLoc, inertia * angVelLoc);
double deltaPE = mass * g * (position.z - PendCSYS.pos.z);
double totalE = transKE + rotKE + deltaPE;
out_energy << simTime << transKE << rotKE << deltaPE << totalE - totalE0 << std::endl;;
// Constraint violations
out_cnstr << simTime << distanceConstraint->GetC() << std::endl;
// Increment output time
outTime += outTimeStep;
}
// Advance simulation by one step
my_system.DoStepDynamics(simTimeStep);
// Increment simulation time
simTime += simTimeStep;
}
// Write output files
out_pos.write_to_file(out_dir + testName + "_CHRONO_Pos.txt", testName + "\n\n");
out_vel.write_to_file(out_dir + testName + "_CHRONO_Vel.txt", testName + "\n\n");
out_acc.write_to_file(out_dir + testName + "_CHRONO_Acc.txt", testName + "\n\n");
out_quat.write_to_file(out_dir + testName + "_CHRONO_Quat.txt", testName + "\n\n");
out_avel.write_to_file(out_dir + testName + "_CHRONO_Avel.txt", testName + "\n\n");
out_aacc.write_to_file(out_dir + testName + "_CHRONO_Aacc.txt", testName + "\n\n");
out_rfrc.write_to_file(out_dir + testName + "_CHRONO_Rforce.txt", testName + "\n\n");
out_rtrq.write_to_file(out_dir + testName + "_CHRONO_Rtorque.txt", testName + "\n\n");
out_energy.write_to_file(out_dir + testName + "_CHRONO_Energy.txt", testName + "\n\n");
out_cnstr.write_to_file(out_dir + testName + "_CHRONO_Constraints.txt", testName + "\n\n");
return true;
}