/// Get the quaternion second derivative from the vector of angular acceleration with a specified in _relative_ coords.
ChQuaternion<double> Qdtdt_from_Arel(const ChVector<double>& a,
const ChQuaternion<double>& q,
const ChQuaternion<double>& q_dt) {
ChQuaternion<double> ret;
ret.Qdtdt_from_Arel(a, q, q_dt);
return ret;
}
// Get the quaternion time derivative from the vector of angular speed, with w specified in _local_ coords.
ChQuaternion<double> Qdt_from_Wrel(const ChVector<double>& w, const ChQuaternion<double>& q) {
ChQuaternion<double> qw;
double half = 0.5;
qw.e0() = 0;
qw.e1() = w.x();
qw.e2() = w.y();
qw.e3() = w.z();
return Qscale(Qcross(q, qw), half); // {q_dt} = 1/2 {q}*{0,w_rel}
}
ChQuaternion<double> Qscale(const ChQuaternion<double>& q, double fact) {
ChQuaternion<double> result;
result.e0() = q.e0() * fact;
result.e1() = q.e1() * fact;
result.e2() = q.e2() * fact;
result.e3() = q.e3() * fact;
return result;
}
// Return the conjugate of the quaternion [s,v1,v2,v3] is [s,-v1,-v2,-v3]
ChQuaternion<double> Qconjugate(const ChQuaternion<double>& q) {
ChQuaternion<double> res;
res.e0() = q.e0();
res.e1() = -q.e1();
res.e2() = -q.e2();
res.e3() = -q.e3();
return (res);
}
// Get the X axis of a coordsystem, given the quaternion which
// represents the alignment of the coordsystem.
ChVector<double> VaxisXfromQuat(const ChQuaternion<double>& quat) {
ChVector<double> res;
res.x() = (pow(quat.e0(), 2) + pow(quat.e1(), 2)) * 2 - 1;
res.y() = ((quat.e1() * quat.e2()) + (quat.e0() * quat.e3())) * 2;
res.z() = ((quat.e1() * quat.e3()) - (quat.e0() * quat.e2())) * 2;
return res;
}
// Get the quaternion from an angle of rotation and an axis, defined in _abs_ coords.
// The axis is supposed to be fixed, i.e. it is constant during rotation.
// The 'axis' vector must be normalized.
ChQuaternion<double> Q_from_AngAxis(double angle, const ChVector<double>& axis) {
ChQuaternion<double> quat;
double halfang;
double sinhalf;
halfang = (angle * 0.5);
sinhalf = sin(halfang);
quat.e0() = cos(halfang);
quat.e1() = axis.x() * sinhalf;
quat.e2() = axis.y() * sinhalf;
quat.e3() = axis.z() * sinhalf;
return (quat);
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChTrackShoeBandBushing::Initialize(std::shared_ptr<ChBodyAuxRef> chassis,
const ChVector<>& location,
const ChQuaternion<>& rotation) {
// Initialize base class (create tread body)
ChTrackShoeBand::Initialize(chassis, location, rotation);
// Cache values calculated from template parameters.
m_seg_length = GetWebLength() / GetNumWebSegments();
m_seg_mass = GetWebMass() / GetNumWebSegments();
m_seg_inertia = GetWebInertia(); //// TODO - properly distribute web inertia
// Express the tread body location and orientation in global frame.
ChVector<> loc = chassis->TransformPointLocalToParent(location);
ChQuaternion<> rot = chassis->GetRot() * rotation;
ChVector<> xdir = rot.GetXaxis();
// Create the required number of web segment bodies
ChVector<> seg_loc = loc + (0.5 * GetToothBaseLength()) * xdir;
for (int is = 0; is < GetNumWebSegments(); is++) {
m_web_segments.push_back(std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody()));
m_web_segments[is]->SetNameString(m_name + "_web_" + std::to_string(is));
m_web_segments[is]->SetPos(seg_loc + ((2 * is + 1) * m_seg_length / 2) * xdir);
m_web_segments[is]->SetRot(rot);
m_web_segments[is]->SetMass(m_seg_mass);
m_web_segments[is]->SetInertiaXX(m_seg_inertia);
chassis->GetSystem()->AddBody(m_web_segments[is]);
// Add contact geometry.
m_web_segments[is]->SetCollide(true);
switch (m_web_segments[is]->GetContactMethod()) {
case ChMaterialSurface::NSC:
m_web_segments[is]->GetMaterialSurfaceNSC()->SetFriction(m_friction);
m_web_segments[is]->GetMaterialSurfaceNSC()->SetRestitution(m_restitution);
break;
case ChMaterialSurface::SMC:
m_web_segments[is]->GetMaterialSurfaceSMC()->SetFriction(m_friction);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetRestitution(m_restitution);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetYoungModulus(m_young_modulus);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetPoissonRatio(m_poisson_ratio);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetKn(m_kn);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetGn(m_gn);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetKt(m_kt);
m_web_segments[is]->GetMaterialSurfaceSMC()->SetGt(m_gt);
break;
}
AddWebContact(m_web_segments[is]);
}
}
void System::DoTimeStep() {
if (mNumCurrentObjects < mNumObjects && mFrameNumber % 100 == 0) {
float x = 1, y = numY, z = 1;
float posX = 0, posY = -8, posZ = 0;
srand(1);
float mass = .01, mu = .5, rest = 0;
ShapeType type = SPHERE;
CHBODYSHAREDPTR mrigidBody;
mNumCurrentObjects += x * y * z;
int mobjNum = 0;
for (int xx = 0; xx < x; xx++) {
for (int yy = 0; yy < y; yy++) {
for (int zz = 0; zz < z; zz++) {
type = CYLINDER;//rand()%2;e
float radius = .5;//(rand()%1000)/3000.0+.05;
ChVector<> mParticlePos((xx - (x - 1) / 2.0) + posX, (yy) + posY, (zz - (z - 1) / 2.0) + posZ);
//mParticlePos += ChVector<> (rand() % 1000 / 10000.0 - .05, rand() % 1000 / 10000.0 - .05, rand() % 1000 / 10000.0 - .05);
ChQuaternion<> quat = ChQuaternion<> (1, 0, 0, 0);// (rand() % 1000 / 1000., rand() % 1000 / 1000., rand() % 1000 / 1000., rand() % 1000 / 1000.);
ChVector<> dim;
ChVector<> lpos(0, 0, 0);
quat.Normalize();
mrigidBody = CHBODYSHAREDPTR(new CHBODY);
InitObject(mrigidBody, mass, mParticlePos * 1.1, quat, mu, mu, rest, true, false, 0, 1);
mrigidBody->SetPos_dt(ChVector<> (0, 0, 0));
switch (type) {
case SPHERE:
dim = ChVector<> (radius, 0, 0);
case ELLIPSOID:
dim = ChVector<> (radius * 1.3, radius, radius * 1.3);
case BOX:
dim = ChVector<> (radius, radius, radius);
case CYLINDER:
dim = ChVector<> (radius, radius*2, radius);
}
AddCollisionGeometry(mrigidBody, type, dim, lpos, quat);
FinalizeObject(mrigidBody);
mobjNum++;
}
}
}
}
mFrameNumber++;
mSystem->DoStepDynamics(mTimeStep);
mCurrentTime += mTimeStep;
GPUSystem->PrintStats();
}
ChQuaternion<double> Q_from_NasaAngles(const ChVector<double>& mang) {
ChQuaternion<double> mq;
double c1 = cos(mang.z() / 2);
double s1 = sin(mang.z() / 2);
double c2 = cos(mang.x() / 2);
double s2 = sin(mang.x() / 2);
double c3 = cos(mang.y() / 2);
double s3 = sin(mang.y() / 2);
double c1c2 = c1 * c2;
double s1s2 = s1 * s2;
mq.e0() = c1c2 * c3 + s1s2 * s3;
mq.e1() = c1c2 * s3 - s1s2 * c3;
mq.e2() = c1 * s2 * c3 + s1 * c2 * s3;
mq.e3() = s1 * c2 * c3 - c1 * s2 * s3;
return mq;
}
void Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
if (fabs(quat.e0()) < 0.99999999) {
double arg = acos(quat.e0());
double invsine = 1 / sin(arg);
ChVector<double> vtemp;
vtemp.x() = invsine * quat.e1();
vtemp.y() = invsine * quat.e2();
vtemp.z() = invsine * quat.e3();
angle = 2 * arg;
axis = Vnorm(vtemp);
} else {
axis.x() = 1;
axis.y() = 0;
axis.z() = 0;
angle = 0;
}
}
// Use convex decomposition for collision detection with the Trelleborg tire
SoilbinWheel(ChSystemNSC* system, ChVector<> mposition, double mass, ChVector<>& inertia) {
ChCollisionModel::SetDefaultSuggestedEnvelope(0.005);
ChCollisionModel::SetDefaultSuggestedMargin(0.004);
// Create the wheel body
wheel = std::make_shared<ChBody>();
wheel->SetPos(mposition);
wheel->SetMass(mass);
wheel->SetInertiaXX(inertia);
wheel->GetMaterialSurfaceNSC()->SetFriction(0.4f);
wheel->SetCollide(true);
// Visualization mesh
auto tireMesh = std::make_shared<ChTriangleMeshConnected>();
tireMesh->LoadWavefrontMesh(GetChronoDataFile("tractor_wheel.obj"), true, true);
auto tireMesh_asset = std::make_shared<ChTriangleMeshShape>();
tireMesh_asset->SetMesh(tireMesh);
wheel->AddAsset(tireMesh_asset);
// Contact mesh
wheel->GetCollisionModel()->ClearModel();
// Describe the (invisible) colliding shape by adding the 'carcass' decomposed shape and the
// 'knobs'. Since these decompositions are only for 1/15th of the wheel, use for() to pattern them.
for (double mangle = 0; mangle < 360.; mangle += (360. / 15.)) {
ChQuaternion<> myrot;
ChStreamInAsciiFile myknobs(GetChronoDataFile("tractor_wheel_knobs.chulls").c_str());
ChStreamInAsciiFile myslice(GetChronoDataFile("tractor_wheel_slice.chulls").c_str());
myrot.Q_from_AngAxis(mangle * (CH_C_PI / 180.), VECT_X);
ChMatrix33<> mm(myrot);
wheel->GetCollisionModel()->AddConvexHullsFromFile(myknobs, ChVector<>(0, 0, 0), mm);
wheel->GetCollisionModel()->AddConvexHullsFromFile(myslice, ChVector<>(0, 0, 0), mm);
}
wheel->GetCollisionModel()->BuildModel();
// Add wheel body to system
system->AddBody(wheel);
}
ChQuaternion<double> Qsub(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
ChQuaternion<double> result;
result.e0() = qa.e0() - qb.e0();
result.e1() = qa.e1() - qb.e1();
result.e2() = qa.e2() - qb.e2();
result.e3() = qa.e3() - qb.e3();
return result;
}
// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion second time derivative,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_dtdt_complete(const ChQuaternion<double>& mq,
const ChQuaternion<double>& mqdt,
const ChVector<double>& qimm_dtdt) {
ChQuaternion<double> mqdtdt;
mqdtdt.e1() = qimm_dtdt.x();
mqdtdt.e2() = qimm_dtdt.y();
mqdtdt.e3() = qimm_dtdt.z();
mqdtdt.e0() = (-mq.e1() * mqdtdt.e1() - mq.e2() * mqdtdt.e2() - mq.e3() * mqdtdt.e3() - mqdt.e0() * mqdt.e0() -
mqdt.e1() * mqdt.e1() - mqdt.e2() * mqdt.e2() - mqdt.e3() * mqdt.e3()) /
mq.e0();
return mqdtdt;
}
ChVector<double> Q_to_NasaAngles(const ChQuaternion<double>& q1) {
ChVector<double> mnasa;
double sqw = q1.e0() * q1.e0();
double sqx = q1.e1() * q1.e1();
double sqy = q1.e2() * q1.e2();
double sqz = q1.e3() * q1.e3();
// heading
mnasa.z() = atan2(2.0 * (q1.e1() * q1.e2() + q1.e3() * q1.e0()), (sqx - sqy - sqz + sqw));
// bank
mnasa.y() = atan2(2.0 * (q1.e2() * q1.e3() + q1.e1() * q1.e0()), (-sqx - sqy + sqz + sqw));
// attitude
mnasa.x() = asin(-2.0 * (q1.e1() * q1.e3() - q1.e2() * q1.e0()));
return mnasa;
}
// 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);
}
// functions
TestMech(ChSystemNSC* system,
std::shared_ptr<ChBody> wheelBody,
double binWidth = 1.0,
double binLength = 2.0,
double weightMass = 100.0,
double springK = 25000,
double springD = 100) {
ChCollisionModel::SetDefaultSuggestedEnvelope(0.003);
ChCollisionModel::SetDefaultSuggestedMargin(0.002);
ChQuaternion<> rot;
rot.Q_from_AngAxis(ChRandom() * CH_C_2PI, VECT_Y);
// *******
// Create a soil bin with planes. bin width = x-dir, bin length = z-dir
// Note: soil bin depth will always be ~ 1m
// *******
double binHeight = 1.0;
double wallWidth = std::min<double>(binWidth, binLength) / 10.0; // wall width = 1/10 of min of bin dims
// create the floor
auto cubeMap = std::make_shared<ChTexture>();
cubeMap->SetTextureFilename(GetChronoDataFile("concrete.jpg"));
floor = std::make_shared<ChBodyEasyBox>(binWidth + wallWidth / 2.0, wallWidth, binLength + wallWidth / 2.0, 1.0, true, true);
floor->SetPos(ChVector<>(0, -0.5 - wallWidth / 2.0, 0));
floor->SetBodyFixed(true);
floor->GetMaterialSurfaceNSC()->SetFriction(0.5);
floor->AddAsset(cubeMap);
system->AddBody(floor);
// add some transparent walls to the soilBin, w.r.t. width, length of bin
wall1 = std::make_shared<ChBodyEasyBox>(wallWidth, binHeight, binLength, 1.0, true, true);
wall1->SetPos(ChVector<>(-binWidth / 2.0 - wallWidth / 2.0, 0, 0));
wall1->SetBodyFixed(true);
system->AddBody(wall1);
wall2 = std::make_shared<ChBodyEasyBox>(wallWidth, binHeight, binLength, 1.0, true, false);
wall2->SetPos(ChVector<>(binWidth / 2.0 + wallWidth / 2.0, 0, 0));
wall2->SetBodyFixed(true);
system->AddBody(wall2);
wall3 = std::make_shared<ChBodyEasyBox>(binWidth + wallWidth / 2.0, binHeight, wallWidth, 1.0, true, false);
wall3->SetPos(ChVector<>(0, 0, -binLength / 2.0 - wallWidth / 2.0));
wall3->SetBodyFixed(true);
system->AddBody(wall3);
// wall 4
wall4 = std::make_shared<ChBodyEasyBox>(binWidth + wallWidth / 2.0, binHeight, wallWidth, 1.0, true, true);
wall4->SetPos(ChVector<>(0, 0, binLength / 2.0 + wallWidth / 2.0));
wall4->SetBodyFixed(true);
system->AddBody(wall4);
// ******
// make a truss, connect it to the wheel via revolute joint
// single rotational DOF will be driven with a user-input for torque
// *****
auto bluMap = std::make_shared<ChTexture>();
bluMap->SetTextureFilename(GetChronoDataFile("blu.png"));
ChVector<> trussCM = wheelBody->GetPos();
truss = std::make_shared<ChBodyEasyBox>(0.2, 0.2, 0.4, 300.0, false, true);
truss->SetPos(trussCM);
truss->SetMass(5.0);
truss->AddAsset(bluMap);
system->AddBody(truss);
// create the revolute joint between the wheel and spindle
spindle = std::make_shared<ChLinkLockRevolute>();
spindle->Initialize(truss, wheelBody, ChCoordsys<>(trussCM, chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_Y)));
system->AddLink(spindle);
// create a torque between the truss and wheel
torqueDriver = std::make_shared<ChLinkEngine>();
torqueDriver->Initialize(truss, wheelBody, ChCoordsys<>(trussCM, chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_Y)));
torqueDriver->Set_eng_mode(ChLinkEngine::ENG_MODE_TORQUE);
system->AddLink(torqueDriver);
// ******
// create a body that will be used as a vehicle weight
ChVector<> weightCM = ChVector<>(trussCM);
weightCM.y() += 1.0; // note: this will determine the spring free length
suspweight = std::make_shared<ChBodyEasyBox>(0.2, 0.4, 0.2, 5000.0, false, true);
suspweight->SetPos(weightCM);
suspweight->SetMass(weightMass);
suspweight->AddAsset(bluMap);
system->AddBody(suspweight);
// create the translational joint between the truss and weight load
auto translational = std::make_shared<ChLinkLockPrismatic>();
translational->Initialize(truss, suspweight,
ChCoordsys<>(trussCM, chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_X)));
system->AddLink(translational);
// create a spring between spindle truss and weight
spring = std::make_shared<ChLinkSpring>();
spring->Initialize(truss, suspweight, false, trussCM, suspweight->GetPos());
spring->Set_SpringK(springK);
spring->Set_SpringR(springD);
system->AddLink(spring);
// create a prismatic constraint between the weight and the ground
auto weightLink = std::make_shared<ChLinkLockOldham>();
weightLink->Initialize(suspweight, floor,
ChCoordsys<>(weightCM, chrono::Q_from_AngAxis(CH_C_PI / 2.0, VECT_Y)));
system->AddLink(weightLink);
}
// Return the product of two quaternions. It is non-commutative (like cross product in vectors).
ChQuaternion<double> Qcross(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
ChQuaternion<double> res;
res.e0() = qa.e0() * qb.e0() - qa.e1() * qb.e1() - qa.e2() * qb.e2() - qa.e3() * qb.e3();
res.e1() = qa.e0() * qb.e1() + qa.e1() * qb.e0() - qa.e3() * qb.e2() + qa.e2() * qb.e3();
res.e2() = qa.e0() * qb.e2() + qa.e2() * qb.e0() + qa.e3() * qb.e1() - qa.e1() * qb.e3();
res.e3() = qa.e0() * qb.e3() + qa.e3() * qb.e0() - qa.e2() * qb.e1() + qa.e1() * qb.e2();
return (res);
}
double Qlength(const ChQuaternion<double>& q) {
return (sqrt(pow(q.e0(), 2) + pow(q.e1(), 2) + pow(q.e2(), 2) + pow(q.e3(), 2)));
}
int main(int argc, char* argv[])
{
// In CHRONO engine, The DLL_CreateGlobals() - DLL_DeleteGlobals(); pair is needed if
// global functions are needed.
DLL_CreateGlobals();
// Create a Chrono::Engine physical system
ChSystem mphysicalSystem;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrApp application(&mphysicalSystem, L"Assets for Irrlicht visualization",core::dimension2d<u32>(800,600),false, true);
// Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
application.AddTypicalLogo();
application.AddTypicalSky();
application.AddTypicalLights();
application.AddTypicalCamera(core::vector3df(0,4,-6));
//
// EXAMPLE 1:
//
// Create a ChBody, and attach some 'assets'
// that define 3D shapes. These shapes can be shown
// by Irrlicht or POV postprocessing, etc...
// Note: these assets are independent from collision shapes!
// Create a rigid body as usual, and add it
// to the physical system:
ChSharedPtr<ChBody> mfloor(new ChBody);
mfloor->SetBodyFixed(true);
// Define a collision shape
mfloor->GetCollisionModel()->ClearModel();
mfloor->GetCollisionModel()->AddBox(10,0.5,10, &ChVector<>(0,-1,0));
mfloor->GetCollisionModel()->BuildModel();
mfloor->SetCollide(true);
// Add body to system
application.GetSystem()->Add(mfloor);
// ==Asset== attach a 'box' shape.
// Note that assets are managed via shared pointer, so they
// can also be shared). Do not forget AddAsset() at the end!
ChSharedPtr<ChBoxShape> mboxfloor(new ChBoxShape);
mboxfloor->GetBoxGeometry().Pos = ChVector<>(0,-1,0);
mboxfloor->GetBoxGeometry().Size = ChVector<>(10,0.5,10);
mfloor->AddAsset(mboxfloor);
// ==Asset== attach color asset.
ChSharedPtr<ChVisualization> mfloorcolor(new ChVisualization);
mfloorcolor->SetColor(ChColor(0.3,0.3,0.6));
mfloor->AddAsset(mfloorcolor);
/*
// ==Asset== IRRLICHT! Add a ChIrrNodeAsset so that Irrlicht will be able
// to 'show' all the assets that we added to the body!
// OTHERWISE: use the application.AssetBind() function as at the end..
ChSharedPtr<ChIrrNodeAsset> mirr_asset_floor(new ChIrrNodeAsset);
mirr_asset_floor->Bind(mfloor, application);
mfloor->AddAsset(mirr_asset_floor);
*/
//
// EXAMPLE 2:
//
// Textures, colors, asset levels with transformations.
// This section shows how to add more advanced types of assets
// and how to group assets in ChAssetLevel containers.
// Create the rigid body as usual (this won't move,
// it is only for visualization tests)
ChSharedPtr<ChBody> mbody(new ChBody);
mbody->SetBodyFixed(true);
application.GetSystem()->Add(mbody);
// ==Asset== Attach a 'sphere' shape
ChSharedPtr<ChSphereShape> msphere(new ChSphereShape);
msphere->GetSphereGeometry().rad = 0.5;
msphere->GetSphereGeometry().center = ChVector<>(-1,0,0);
mbody->AddAsset(msphere);
// ==Asset== Attach also a 'box' shape
ChSharedPtr<ChBoxShape> mbox(new ChBoxShape);
mbox->GetBoxGeometry().Pos = ChVector<>(1,1,0);
mbox->GetBoxGeometry().Size = ChVector<>(0.3,0.5,0.1);
mbody->AddAsset(mbox);
// ==Asset== Attach also a 'cylinder' shape
ChSharedPtr<ChCylinderShape> mcyl(new ChCylinderShape);
mcyl->GetCylinderGeometry().p1 = ChVector<>(2,-0.2,0);
mcyl->GetCylinderGeometry().p2 = ChVector<>(2.2,0.5,0);
mcyl->GetCylinderGeometry().rad = 0.3;
mbody->AddAsset(mcyl);
// ==Asset== Attach color. To set colors for all assets
// in the same level, just add this:
ChSharedPtr<ChVisualization> mvisual(new ChVisualization);
mvisual->SetColor(ChColor(0.9,0.4,0.2));
mbody->AddAsset(mvisual);
// ==Asset== Attach a level that contains other assets.
// Note: a ChAssetLevel can define a rotation/translation respect to paren level,
// Note: a ChAssetLevel can contain colors or textures: if any, they affect only objects in the level.
ChSharedPtr<ChAssetLevel> mlevelA(new ChAssetLevel);
// ==Asset== Attach, in this level, a 'Wavefront mesh' asset,
// referencing a .obj file:
ChSharedPtr<ChObjShapeFile> mobjmesh(new ChObjShapeFile);
mobjmesh->SetFilename("../data/forklift_body.obj");
mlevelA->AddAsset(mobjmesh);
// ==Asset== Attach also a texture, that will affect only the
// assets in mlevelA:
ChSharedPtr<ChTexture> mtexture(new ChTexture);
mtexture->SetTextureFilename("../data/bluwhite.png");
mlevelA->AddAsset(mtexture);
// Change the position of mlevelA, thus moving also its sub-assets:
mlevelA->GetFrame().SetPos(ChVector<>(0,0,2));
mbody->AddAsset(mlevelA);
// ==Asset== Attach sub level, then add to it an array of sub-levels,
// each rotated, and each containing a displaced box, thus making a
// spiral of cubes
ChSharedPtr<ChAssetLevel> mlevelB(new ChAssetLevel);
for (int j = 0; j<20; j++)
{
// ==Asset== the sub sub level..
ChSharedPtr<ChAssetLevel> mlevelC(new ChAssetLevel);
// ==Asset== the contained box..
ChSharedPtr<ChBoxShape> msmallbox(new ChBoxShape);
msmallbox->GetBoxGeometry().Pos = ChVector<>(0.4,0,0);
msmallbox->GetBoxGeometry().Size = ChVector<>(0.1,0.1,0.01);
mlevelC->AddAsset(msmallbox);
ChQuaternion<> mrot;
mrot.Q_from_AngAxis(j*21*CH_C_DEG_TO_RAD, ChVector<>(0,1,0));
mlevelC->GetFrame().SetRot(mrot);
mlevelC->GetFrame().SetPos(ChVector<>(0,j*0.02,0));
mlevelB->AddAsset(mlevelC);
}
mbody->AddAsset(mlevelB);
// ==Asset== Attach a video camera. This will be used by Irrlicht,
// or POVray postprocessing, etc. Note that a camera can also be
// put in a moving object.
ChSharedPtr<ChCamera> mcamera(new ChCamera);
mcamera->SetAngle(50);
mcamera->SetPosition(ChVector<>(-3,4,-5));
mcamera->SetAimPoint(ChVector<>(0,1,0));
mbody->AddAsset(mcamera);
/*
// ==Asset== IRRLICHT! Add a ChIrrNodeAsset so that Irrlicht will be able
// to 'show' all the assets that we added to the body!
// OTHERWISE: use the application.AssetBind() function as at the end..
ChSharedPtr<ChIrrNodeAsset> mirr_asset(new ChIrrNodeAsset);
mirr_asset->Bind(mbody, application);
mbody->AddAsset(mirr_asset);
*/
//
// EXAMPLE 3:
//
// Create a ChParticleClones cluster, and attach 'assets'
// that define a single "sample" 3D shape. This will be shown
// N times in Irrlicht.
// Create the ChParticleClones, populate it with some random particles,
// and add it to physical system:
ChSharedPtr<ChParticlesClones> mparticles(new ChParticlesClones);
// Note: coll. shape, if needed, must be specified before creating particles
mparticles->GetCollisionModel()->ClearModel();
mparticles->GetCollisionModel()->AddSphere(0.05);
mparticles->GetCollisionModel()->BuildModel();
mparticles->SetCollide(true);
// Create the random particles
for (int np=0; np<100; ++np)
mparticles->AddParticle(ChCoordsys<>(ChVector<>(ChRandom()-2,1.5,ChRandom()+2)));
// Do not forget to add the particle cluster to the system:
application.GetSystem()->Add(mparticles);
// ==Asset== Attach a 'sphere' shape asset.. it will be used as a sample
// shape to display all particles when rendering in 3D!
ChSharedPtr<ChSphereShape> mspherepart(new ChSphereShape);
mspherepart->GetSphereGeometry().rad = 0.05;
mparticles->AddAsset(mspherepart);
/*
// ==Asset== IRRLICHT! Add a ChIrrNodeAsset so that Irrlicht will be able
// to 'show' all the assets that we added to the body!
// OTHERWISE: use the application.AssetBind() function as at the end!
ChSharedPtr<ChIrrNodeAsset> mirr_assetpart(new ChIrrNodeAsset);
mirr_assetpart->Bind(mparticles, application);
mparticles->AddAsset(mirr_assetpart);
*/
////////////////////////
// ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
// in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
// If you need a finer control on which item really needs a visualization proxy in
// Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
application.AssetBindAll();
// ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
// that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
application.AssetUpdateAll();
//
// THE SOFT-REAL-TIME CYCLE
//
application.SetStepManage(true);
application.SetTimestep(0.01);
application.SetTryRealtime(true);
while(application.GetDevice()->run())
{
application.BeginScene();
application.DrawAll();
application.DoStep();
application.EndScene();
}
// Remember this at the end of the program, if you started
// with DLL_CreateGlobals();
DLL_DeleteGlobals();
return 0;
}
// Check if quaternion is not null
bool Qnotnull(const ChQuaternion<double>& qa) {
return (qa.e0() != 0) || (qa.e1() != 0) || (qa.e2() != 0) || (qa.e3() != 0);
}
// =============================================================================
//
// 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;
}
// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_complete(const ChVector<double>& qimm) {
ChQuaternion<double> mq;
mq.e1() = qimm.x();
mq.e2() = qimm.y();
mq.e3() = qimm.z();
mq.e0() = sqrt(1 - mq.e1() * mq.e1() - mq.e2() * mq.e2() - mq.e3() * mq.e3());
return mq;
}
