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