int ChLcpSystemDescriptor::BuildDiVector( ChMatrix<>& Dvector ) { n_q=CountActiveVariables(); n_c=CountActiveConstraints(); Dvector.Reset(n_q+n_c,1); // fast! Reset() method does not realloc if size doesn't change // Fills the 'f' vector part #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) { Dvector.PasteMatrix(&vvariables[iv]->Get_fb(), vvariables[iv]->GetOffset(), 0); } } // Fill the '-b' vector (with flipped sign!) #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { Dvector(vconstraints[ic]->GetOffset() + n_q) = - vconstraints[ic]->Get_b_i(); } } return n_q+n_c; }
int ChLcpSystemDescriptor::FromUnknownsToVector( ChMatrix<>& mvector, bool resize_vector ) { // Count active variables & constraints and resize vector if necessary n_q= CountActiveVariables(); n_c= CountActiveConstraints(); if (resize_vector) { mvector.Resize(n_q+n_c, 1); } // Fill the first part of vector, x.q ,with variables q #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) { mvector.PasteMatrix(&vvariables[iv]->Get_qb(), vvariables[iv]->GetOffset(), 0); } } // Fill the second part of vector, x.l, with constraint multipliers -l (with flipped sign!) #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { mvector(vconstraints[ic]->GetOffset() + n_q) = -vconstraints[ic]->Get_l_i(); } } return n_q+n_c; }
// ----------------------------------------------------------------------------- // ----------------------------------------------------------------------------- void ChDoubleWishboneReduced::LogConstraintViolations(VehicleSide side) { // Revolute joint { ChMatrix<>* C = m_revolute[side]->GetC(); GetLog() << "Spindle revolute "; GetLog() << " " << C->GetElement(0, 0) << " "; GetLog() << " " << C->GetElement(1, 0) << " "; GetLog() << " " << C->GetElement(2, 0) << " "; GetLog() << " " << C->GetElement(3, 0) << " "; GetLog() << " " << C->GetElement(4, 0) << "\n"; } // Distance constraints GetLog() << "UCA front distance "; GetLog() << " " << m_distUCA_F[side]->GetCurrentDistance() - m_distUCA_F[side]->GetImposedDistance() << "\n"; GetLog() << "UCA back distance "; GetLog() << " " << m_distUCA_B[side]->GetCurrentDistance() - m_distUCA_B[side]->GetImposedDistance() << "\n"; GetLog() << "LCA front distance "; GetLog() << " " << m_distLCA_F[side]->GetCurrentDistance() - m_distLCA_F[side]->GetImposedDistance() << "\n"; GetLog() << "LCA back distance "; GetLog() << " " << m_distLCA_B[side]->GetCurrentDistance() - m_distLCA_B[side]->GetImposedDistance() << "\n"; GetLog() << "Tierod distance "; GetLog() << " " << m_distTierod[side]->GetCurrentDistance() - m_distTierod[side]->GetImposedDistance() << "\n"; }
// ----------------------------------------------------------------------------- // ----------------------------------------------------------------------------- void ChPitmanArm::LogConstraintViolations() { // Revolute joint ////{ //// ChMatrix<>* C = m_revolute->GetC(); //// GetLog() << "Revolute "; //// GetLog() << " " << C->GetElement(0, 0) << " "; //// GetLog() << " " << C->GetElement(1, 0) << " "; //// GetLog() << " " << C->GetElement(2, 0) << " "; //// GetLog() << " " << C->GetElement(3, 0) << " "; //// GetLog() << " " << C->GetElement(4, 0) << "\n"; ////} // Universal joint { ChMatrix<>* C = m_universal->GetC(); GetLog() << "Universal "; GetLog() << " " << C->GetElement(0, 0) << " "; GetLog() << " " << C->GetElement(1, 0) << " "; GetLog() << " " << C->GetElement(2, 0) << " "; GetLog() << " " << C->GetElement(3, 0) << "\n"; } // Revolute-spherical joint { ChMatrix<>* C = m_revsph->GetC(); GetLog() << "Revolute-spherical "; GetLog() << " " << C->GetElement(0, 0) << " "; GetLog() << " " << C->GetElement(1, 0) << "\n"; } }
// Computes the product of the mass matrix by a // vector, and set in result: result = [Mb]*vect void ChVariablesNode::Compute_inc_Mb_v(ChMatrix<double>& result, const ChMatrix<double>& vect) const { assert(result.GetRows() == vect.GetRows()); assert(vect.GetRows() == Get_ndof()); // optimized unrolled operations result(0) += mass * vect(0); result(1) += mass * vect(1); result(2) += mass * vect(2); }
// Computes the product of the corresponding block in the // system matrix (ie. the mass matrix) by 'vect', scale by c_a, and add to 'result'. // NOTE: the 'vect' and 'result' vectors must already have // the size of the total variables&constraints in the system; the procedure // will use the ChVariable offsets (that must be already updated) to know the // indexes in result and vect. void ChVariablesNode::MultiplyAndAdd(ChMatrix<double>& result, const ChMatrix<double>& vect, const double c_a) const { assert(result.GetColumns() == 1 && vect.GetColumns() == 1); // optimized unrolled operations double scaledmass = c_a * mass; result(this->offset) += scaledmass * vect(this->offset); result(this->offset + 1) += scaledmass * vect(this->offset + 1); result(this->offset + 2) += scaledmass * vect(this->offset + 2); }
// ----------------------------------------------------------------------------- // ----------------------------------------------------------------------------- void ChRoadWheel::LogConstraintViolations() { ChMatrix<>* C = m_revolute->GetC(); GetLog() << " Road-wheel revolute\n"; GetLog() << " " << C->GetElement(0, 0) << " "; GetLog() << " " << C->GetElement(1, 0) << " "; GetLog() << " " << C->GetElement(2, 0) << " "; GetLog() << " " << C->GetElement(3, 0) << " "; GetLog() << " " << C->GetElement(4, 0) << "\n"; }
// ----------------------------------------------------------------------------- // ----------------------------------------------------------------------------- void ChLinearDamperRWAssembly::LogConstraintViolations() { ChMatrix<>* C = m_revolute->GetC(); GetLog() << " Arm-chassis revolute\n"; GetLog() << " " << C->GetElement(0, 0) << " "; GetLog() << " " << C->GetElement(1, 0) << " "; GetLog() << " " << C->GetElement(2, 0) << " "; GetLog() << " " << C->GetElement(3, 0) << " "; GetLog() << " " << C->GetElement(4, 0) << "\n"; m_road_wheel->LogConstraintViolations(); }
// Computes the product of the mass matrix by a // vector, and set in result: result = [Mb]*vect void ChVariablesBodySharedMass::Compute_inc_Mb_v(ChMatrix<double>& result, const ChMatrix<double>& vect) const { assert(result.GetRows() == Get_ndof()); assert(vect.GetRows() == Get_ndof()); // optimized unrolled operations result(0) += sharedmass->mass * vect(0); result(1) += sharedmass->mass * vect(1); result(2) += sharedmass->mass * vect(2); result(3) += (sharedmass->inertia(0, 0) * vect(3) + sharedmass->inertia(0, 1) * vect(4) + sharedmass->inertia(0, 2) * vect(5)); result(4) += (sharedmass->inertia(1, 0) * vect(3) + sharedmass->inertia(1, 1) * vect(4) + sharedmass->inertia(1, 2) * vect(5)); result(5) += (sharedmass->inertia(2, 0) * vect(3) + sharedmass->inertia(2, 1) * vect(4) + sharedmass->inertia(2, 2) * vect(5)); }
ChMapMatrix::ChMapMatrix(const ChMatrix<>& mat) { m_num_rows = mat.GetRows(); m_num_cols = mat.GetColumns(); m_rows.resize(mat.GetRows()); for (int ir = 0; ir < m_num_rows; ir++) { for (int ic = 0; ic < m_num_cols; ic++) { double val = mat.GetElement(ir, ic); if (val != 0) { ChMapMatrix::SetElement(ir, ic, val); } } } m_CSR_current = false; }
int ChLcpSystemDescriptor::BuildFbVector( ChMatrix<>& Fvector ///< matrix which will contain the entire vector of 'f' ) { n_q=CountActiveVariables(); Fvector.Reset(n_q,1); // fast! Reset() method does not realloc if size doesn't change // Fills the 'f' vector #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) { Fvector.PasteMatrix(&vvariables[iv]->Get_fb(), vvariables[iv]->GetOffset(), 0); } } return this->n_q; }
// Add the diagonal of the mass matrix scaled by c_a, to 'result'. // NOTE: the 'result' vector must already have the size of system unknowns, ie // the size of the total variables&constraints in the system; the procedure // will use the ChVariable offset (that must be already updated) as index. void ChVariablesBodySharedMass::DiagonalAdd(ChMatrix<double>& result, const double c_a) const { assert(result.GetColumns() == 1); result(this->offset + 0) += c_a * sharedmass->mass; result(this->offset + 1) += c_a * sharedmass->mass; result(this->offset + 2) += c_a * sharedmass->mass; result(this->offset + 3) += c_a * sharedmass->inertia(0, 0); result(this->offset + 4) += c_a * sharedmass->inertia(1, 1); result(this->offset + 5) += c_a * sharedmass->inertia(2, 2); }
// Computes the product of the corresponding block in the // system matrix (ie. the mass matrix) by 'vect', scale by c_a, and add to 'result'. // NOTE: the 'vect' and 'result' vectors must already have // the size of the total variables&constraints in the system; the procedure // will use the ChVariable offsets (that must be already updated) to know the // indexes in result and vect. void ChVariablesBodySharedMass::MultiplyAndAdd(ChMatrix<double>& result, const ChMatrix<double>& vect, const double c_a) const { assert(result.GetColumns() == 1 && vect.GetColumns() == 1); // optimized unrolled operations double q0 = vect(this->offset + 0); double q1 = vect(this->offset + 1); double q2 = vect(this->offset + 2); double q3 = vect(this->offset + 3); double q4 = vect(this->offset + 4); double q5 = vect(this->offset + 5); double scaledmass = c_a * sharedmass->mass; result(this->offset + 0) += scaledmass * q0; result(this->offset + 1) += scaledmass * q1; result(this->offset + 2) += scaledmass * q2; result(this->offset + 3) += c_a * (sharedmass->inertia(0, 0) * q3 + sharedmass->inertia(0, 1) * q4 + sharedmass->inertia(0, 2) * q5); result(this->offset + 4) += c_a * (sharedmass->inertia(1, 0) * q3 + sharedmass->inertia(1, 1) * q4 + sharedmass->inertia(1, 2) * q5); result(this->offset + 5) += c_a * (sharedmass->inertia(2, 0) * q3 + sharedmass->inertia(2, 1) * q4 + sharedmass->inertia(2, 2) * q5); }
int ChLcpSystemDescriptor::FromVectorToVariables( ChMatrix<>& mvector ) { #ifdef CH_DEBUG n_q= CountActiveVariables(); assert(n_q == mvector.GetRows()); assert(mvector.GetColumns()==1); #endif // fetch from the vector #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) { vvariables[iv]->Get_qb().PasteClippedMatrix(&mvector, vvariables[iv]->GetOffset(), 0, vvariables[iv]->Get_ndof(),1, 0,0); } } return n_q; }
int ChLcpSystemDescriptor::FromVectorToUnknowns( ChMatrix<>& mvector ) { n_q= CountActiveVariables(); n_c= CountActiveConstraints(); #ifdef CH_DEBUG assert((n_q+n_c) == mvector.GetRows()); assert(mvector.GetColumns()==1); #endif // fetch from the first part of vector (x.q = q) #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { //int rank = CHOMPfunctions::GetThreadNum(); //int count = CHOMPfunctions::GetNumThreads(); //GetLog() << " FromVectorToUnknowns: thread " << rank << " on " << count << "\n"; //GetLog().Flush(); if (vvariables[iv]->IsActive()) { vvariables[iv]->Get_qb().PasteClippedMatrix(&mvector, vvariables[iv]->GetOffset(), 0, vvariables[iv]->Get_ndof(),1, 0,0); } } // fetch from the second part of vector (x.l = -l), with flipped sign! #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { vconstraints[ic]->Set_l_i( - mvector( vconstraints[ic]->GetOffset() + n_q )); } } return n_q+n_c; }
int ChLcpSystemDescriptor::FromVectorToConstraints( ChMatrix<>& mvector ) { n_c=CountActiveConstraints(); #ifdef CH_DEBUG assert(n_c == mvector.GetRows()); assert(mvector.GetColumns()==1); #endif // Fill the vector #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { vconstraints[ic]->Set_l_i( mvector(vconstraints[ic]->GetOffset()) ); } } return n_c; }
int ChLcpSystemDescriptor::FromVariablesToVector( ChMatrix<>& mvector, bool resize_vector ) { // Count active variables and resize vector if necessary if (resize_vector) { n_q= CountActiveVariables(); mvector.Resize(n_q, 1); } // Fill the vector // #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) { mvector.PasteMatrix(&vvariables[iv]->Get_qb(), vvariables[iv]->GetOffset(), 0); } } return n_q; }
void ChLcpKstiffnessGeneric::DiagonalAdd(ChMatrix<double>& result) { assert(result.GetColumns()==1); int kio =0; for (unsigned int iv = 0; iv< this->GetNvars(); iv++) { int io = this->GetVariableN(iv)->GetOffset(); int in = this->GetVariableN(iv)->Get_ndof(); for (int r = 0; r < in; r++) { //GetLog() << "Summing" << result(io+r) << " to " << (*this->K)(kio+r,kio+r) << "\n"; result(io+r) += (*this->K)(kio+r,kio+r); } kio += in; } }
int ChLcpSystemDescriptor::BuildBiVector( ChMatrix<>& Bvector ///< matrix which will contain the entire vector of 'b' ) { n_c=CountActiveConstraints(); Bvector.Resize(n_c, 1); // Fill the 'b' vector #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { Bvector(vconstraints[ic]->GetOffset()) = vconstraints[ic]->Get_b_i(); } } return n_c; }
int ChLcpSystemDescriptor::BuildDiagonalVector( ChMatrix<>& Diagonal_vect ///< matrix which will contain the entire vector of terms on M and E diagonal ) { n_q=CountActiveVariables(); n_c=CountActiveConstraints(); Diagonal_vect.Reset(n_q+n_c,1); // fast! Reset() method does not realloc if size doesn't change // Fill the diagonal values given by stiffness blocks, if any // (This cannot be easily parallelized because of possible write concurrency). for (int is = 0; is< (int)vstiffness.size(); is++) { vstiffness[is]->DiagonalAdd(Diagonal_vect); } // Get the 'M' diagonal terms #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) { vvariables[iv]->DiagonalAdd(Diagonal_vect); } } // Get the 'E' diagonal terms (note the sign: E_i = -cfm_i ) #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { Diagonal_vect(vconstraints[ic]->GetOffset() + n_q) = - vconstraints[ic]->Get_cfm_i(); } } return n_q+n_c; }
int ChLcpSystemDescriptor::FromConstraintsToVector( ChMatrix<>& mvector, bool resize_vector ) { // Count active constraints and resize vector if necessary if (resize_vector) { n_c=CountActiveConstraints(); mvector.Resize(n_c, 1); } // Fill the vector #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic< (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { mvector(vconstraints[ic]->GetOffset()) = vconstraints[ic]->Get_l_i(); } } return n_c; }
// Add the diagonal of the mass matrix scaled by c_a, to 'result'. // NOTE: the 'result' vector must already have the size of system unknowns, ie // the size of the total variables&constraints in the system; the procedure // will use the ChVariable offset (that must be already updated) as index. void ChVariablesShaft::DiagonalAdd(ChMatrix<double>& result, const double c_a) const { assert(result.GetColumns() == 1); result(this->offset) += c_a * m_inertia; }
// Computes the product of the corresponding block in the // system matrix (ie. the mass matrix) by 'vect', scale by c_a, and add to 'result'. // NOTE: the 'vect' and 'result' vectors must already have // the size of the total variables&constraints in the system; the procedure // will use the ChVariable offsets (that must be already updated) to know the // indexes in result and vect. void ChVariablesShaft::MultiplyAndAdd(ChMatrix<double>& result, const ChMatrix<double>& vect, const double c_a) const { assert(result.GetColumns() == 1 && vect.GetColumns() == 1); result(this->offset) += c_a * m_inertia * vect(this->offset); }
// Computes the product of the mass matrix by a // vector, and set in result: result = [Mb]*vect void ChVariablesShaft::Compute_inc_Mb_v(ChMatrix<double>& result, const ChMatrix<double>& vect) const { assert(result.GetRows() == vect.GetRows()); assert(vect.GetRows() == Get_ndof()); result(0) += m_inertia * vect(0); }
void ChLcpSystemDescriptor::SystemProduct( ChMatrix<>& result, ///< matrix which contains the result of matrix by x ChMatrix<>* x ///< optional matrix with the vector to be multiplied (if null, use current l_i and q) // std::vector<bool>* enabled=0 ///< optional: vector of enable flags, one per scalar constraint. true=enable, false=disable (skip) ) { n_q = this->CountActiveVariables(); n_c = this->CountActiveConstraints(); ChMatrix<>* x_ql = 0; ChMatrix<>* vect; if (x) { #ifdef CH_DEBUG assert(x->GetRows() == n_q+n_c); assert(x->GetColumns()== 1); #endif vect = x; } else { x_ql = new ChMatrixDynamic<double>(n_q+n_c,1); vect = x_ql; this->FromUnknownsToVector(*vect); } result.Reset(n_q+n_c,1); // fast! Reset() method does not realloc if size doesn't change // 1) First row: result.q part = [M + K]*x.q + [Cq']*x.l // 1.1) do M*x.q #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) if (vvariables[iv]->IsActive()) { vvariables[iv]->MultiplyAndAdd(result,*x); } // 1.2) add also K*x.q (NON straight parallelizable - risk of concurrency in writing) for (int ik = 0; ik< (int)vstiffness.size(); ik++) { vstiffness[ik]->MultiplyAndAdd(result,*x); } // 1.3) add also [Cq]'*x.l (NON straight parallelizable - risk of concurrency in writing) for (int ic = 0; ic < (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { vconstraints[ic]->MultiplyTandAdd(result, (*x)(vconstraints[ic]->GetOffset()+n_q)); } } // 2) Second row: result.l part = [C_q]*x.q + [E]*x.l #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic < (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { int s_c = vconstraints[ic]->GetOffset() + n_q; vconstraints[ic]->MultiplyAndAdd(result(s_c), (*x)); // result.l_i += [C_q_i]*x.q result(s_c) -= vconstraints[ic]->Get_cfm_i()* (*x)(s_c); // result.l_i += [E]*x.l_i NOTE: cfm = -E } } // if a temp vector has been created because x was not provided, then delete it if (x_ql) delete x_ql; }
// Add the diagonal of the mass matrix scaled by c_a, to 'result'. // NOTE: the 'result' vector must already have the size of system unknowns, ie // the size of the total variables&constraints in the system; the procedure // will use the ChVariable offset (that must be already updated) as index. void ChVariablesNode::DiagonalAdd(ChMatrix<double>& result, const double c_a) const { assert(result.GetColumns() == 1); result(this->offset) += c_a * mass; result(this->offset + 1) += c_a * mass; result(this->offset + 2) += c_a * mass; }
int main(int argc, char* argv[]) { // Create output directories. if (ChFileutils::MakeDirectory(out_dir.c_str()) < 0) { cout << "Error creating directory " << out_dir << endl; return 1; } if (ChFileutils::MakeDirectory(pov_dir.c_str()) < 0) { cout << "Error creating directory " << pov_dir << endl; return 1; } // ------------- // Create system // ------------- #ifdef USE_DEM cout << "Create DEM system" << endl; ChSystemParallelDEM* msystem = new ChSystemParallelDEM(); #else cout << "Create DVI system" << endl; ChSystemParallelDVI* msystem = new ChSystemParallelDVI(); #endif msystem->Set_G_acc(ChVector<>(0, 0, -gravity)); // Set number of threads. int max_threads = omp_get_num_procs(); if (threads > max_threads) threads = max_threads; msystem->SetParallelThreadNumber(threads); omp_set_num_threads(threads); cout << "Using " << threads << " threads" << endl; msystem->GetSettings()->max_threads = threads; msystem->GetSettings()->perform_thread_tuning = thread_tuning; // Edit system settings msystem->GetSettings()->solver.use_full_inertia_tensor = false; msystem->GetSettings()->solver.tolerance = tolerance; msystem->GetSettings()->solver.max_iteration_bilateral = max_iteration_bilateral; msystem->GetSettings()->solver.clamp_bilaterals = clamp_bilaterals; msystem->GetSettings()->solver.bilateral_clamp_speed = bilateral_clamp_speed; #ifdef USE_DEM msystem->GetSettings()->collision.narrowphase_algorithm = NARROWPHASE_R; msystem->GetSettings()->solver.contact_force_model = contact_force_model; msystem->GetSettings()->solver.tangential_displ_mode = tangential_displ_mode; #else msystem->GetSettings()->solver.solver_mode = SLIDING; msystem->GetSettings()->solver.max_iteration_normal = max_iteration_normal; msystem->GetSettings()->solver.max_iteration_sliding = max_iteration_sliding; msystem->GetSettings()->solver.max_iteration_spinning = max_iteration_spinning; msystem->GetSettings()->solver.alpha = 0; msystem->GetSettings()->solver.contact_recovery_speed = contact_recovery_speed; msystem->SetMaxPenetrationRecoverySpeed(contact_recovery_speed); msystem->ChangeSolverType(APGDREF); msystem->GetSettings()->collision.collision_envelope = 0.05 * r_g; #endif msystem->GetSettings()->collision.bins_per_axis = I3(10, 10, 10); // -------------- // Problem set up // -------------- // Depending on problem type: // - Select end simulation time // - Select output FPS // - Create / load objects double time_min = 0; double time_end; int out_fps; ChSharedPtr<ChBody> ground; ChSharedPtr<ChBody> loadPlate; ChSharedPtr<ChLinkLockPrismatic> prismatic; ChSharedPtr<ChLinkLinActuator> actuator; switch (problem) { case SETTLING: { time_min = time_settling_min; time_end = time_settling_max; out_fps = out_fps_settling; // Create the mechanism bodies (all fixed). CreateMechanismBodies(msystem); // Grab handles to mechanism bodies (must increase ref counts) ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0)); loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1)); msystem->Get_bodylist()->at(0)->AddRef(); msystem->Get_bodylist()->at(1)->AddRef(); // Create granular material. int num_particles = CreateGranularMaterial(msystem); cout << "Granular material: " << num_particles << " particles" << endl; break; } case PRESSING: { time_min = time_pressing_min; time_end = time_pressing_max; out_fps = out_fps_pressing; // Create bodies from checkpoint file. cout << "Read checkpoint data from " << settled_ckpnt_file; utils::ReadCheckpoint(msystem, settled_ckpnt_file); cout << " done. Read " << msystem->Get_bodylist()->size() << " bodies." << endl; // Grab handles to mechanism bodies (must increase ref counts) ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0)); loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1)); msystem->Get_bodylist()->at(0)->AddRef(); msystem->Get_bodylist()->at(1)->AddRef(); // Move the load plate just above the granular material. double highest, lowest; FindHeightRange(msystem, lowest, highest); ChVector<> pos = loadPlate->GetPos(); double z_new = highest + 1.01 * r_g; loadPlate->SetPos(ChVector<>(pos.x, pos.y, z_new)); // Add collision geometry to plate loadPlate->GetCollisionModel()->ClearModel(); utils::AddBoxGeometry(loadPlate.get_ptr(), ChVector<>(hdimX_p, hdimY_p, hdimZ_p), ChVector<>(0, 0, hdimZ_p)); loadPlate->GetCollisionModel()->BuildModel(); // If using an actuator, connect the load plate and get a handle to the actuator. if (use_actuator) { ConnectLoadPlate(msystem, ground, loadPlate); prismatic = msystem->SearchLink("prismatic").StaticCastTo<ChLinkLockPrismatic>(); actuator = msystem->SearchLink("actuator").StaticCastTo<ChLinkLinActuator>(); } // Release the load plate. loadPlate->SetBodyFixed(!use_actuator); break; } case TESTING: { time_end = time_testing; out_fps = out_fps_testing; // For TESTING only, increse shearing velocity. desiredVelocity = 0.5; // Create the mechanism bodies (all fixed). CreateMechanismBodies(msystem); // Create the test ball. CreateBall(msystem); // Grab handles to mechanism bodies (must increase ref counts) ground = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(0)); loadPlate = ChSharedPtr<ChBody>(msystem->Get_bodylist()->at(1)); msystem->Get_bodylist()->at(0)->AddRef(); msystem->Get_bodylist()->at(1)->AddRef(); // Move the load plate just above the test ball. ChVector<> pos = loadPlate->GetPos(); double z_new = 2.1 * radius_ball; loadPlate->SetPos(ChVector<>(pos.x, pos.y, z_new)); // Add collision geometry to plate loadPlate->GetCollisionModel()->ClearModel(); utils::AddBoxGeometry(loadPlate.get_ptr(), ChVector<>(hdimX_p, hdimY_p, hdimZ_p), ChVector<>(0, 0, hdimZ_p)); loadPlate->GetCollisionModel()->BuildModel(); // If using an actuator, connect the shear box and get a handle to the actuator. if (use_actuator) { ConnectLoadPlate(msystem, ground, loadPlate); actuator = msystem->SearchLink("actuator").StaticCastTo<ChLinkLinActuator>(); } // Release the shear box when using an actuator. loadPlate->SetBodyFixed(!use_actuator); break; } } // ---------------------- // Perform the simulation // ---------------------- // Set number of simulation steps and steps between successive output int num_steps = (int)std::ceil(time_end / time_step); int out_steps = (int)std::ceil((1.0 / time_step) / out_fps); int write_steps = (int)std::ceil((1.0 / time_step) / write_fps); // Initialize counters double time = 0; int sim_frame = 0; int out_frame = 0; int next_out_frame = 0; double exec_time = 0; int num_contacts = 0; double max_cnstr_viol[2] = {0, 0}; // Circular buffer with highest particle location // (only used for SETTLING or PRESSING) int buffer_size = std::ceil(time_min / time_step); std::valarray<double> hdata(0.0, buffer_size); // Create output files ChStreamOutAsciiFile statsStream(stats_file.c_str()); ChStreamOutAsciiFile sinkageStream(sinkage_file.c_str()); sinkageStream.SetNumFormat("%16.4e"); #ifdef CHRONO_PARALLEL_HAS_OPENGL opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance(); gl_window.Initialize(1280, 720, "Pressure Sinkage Test", msystem); gl_window.SetCamera(ChVector<>(0, -10 * hdimY, hdimZ), ChVector<>(0, 0, hdimZ), ChVector<>(0, 0, 1)); gl_window.SetRenderMode(opengl::WIREFRAME); #endif // Loop until reaching the end time... while (time < time_end) { // Current position and velocity of the shear box ChVector<> pos_old = loadPlate->GetPos(); ChVector<> vel_old = loadPlate->GetPos_dt(); // Calculate minimum and maximum particle heights double highest, lowest; FindHeightRange(msystem, lowest, highest); // If at an output frame, write PovRay file and print info if (sim_frame == next_out_frame) { cout << "------------ Output frame: " << out_frame + 1 << endl; cout << " Sim frame: " << sim_frame << endl; cout << " Time: " << time << endl; cout << " Load plate pos: " << pos_old.z << endl; cout << " Lowest point: " << lowest << endl; cout << " Highest point: " << highest << endl; cout << " Execution time: " << exec_time << endl; // Save PovRay post-processing data. if (write_povray_data) { char filename[100]; sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), out_frame + 1); utils::WriteShapesPovray(msystem, filename, false); } // Create a checkpoint from the current state. if (problem == SETTLING || problem == PRESSING) { cout << " Write checkpoint data " << flush; if (problem == SETTLING) utils::WriteCheckpoint(msystem, settled_ckpnt_file); else utils::WriteCheckpoint(msystem, pressed_ckpnt_file); cout << msystem->Get_bodylist()->size() << " bodies" << endl; } // Increment counters out_frame++; next_out_frame += out_steps; } // Check for early termination of a settling phase. if (problem == SETTLING) { // Store maximum particle height in circular buffer hdata[sim_frame % buffer_size] = highest; // Check variance of data in circular buffer if (time > time_min) { double mean_height = hdata.sum() / buffer_size; std::valarray<double> x = hdata - mean_height; double var = std::sqrt((x * x).sum() / buffer_size); // Consider the material settled when the variance is below the // specified fraction of a particle radius if (var < settling_tol * r_g) { cout << "Granular material settled... time = " << time << endl; break; } } } // Advance simulation by one step #ifdef CHRONO_PARALLEL_HAS_OPENGL if (gl_window.Active()) { gl_window.DoStepDynamics(time_step); gl_window.Render(); } else break; #else msystem->DoStepDynamics(time_step); #endif TimingOutput(msystem); // Record stats about the simulation if (sim_frame % write_steps == 0) { // write stat info int numIters = msystem->data_manager->measures.solver.maxd_hist.size(); double residual = 0; if (numIters) residual = msystem->data_manager->measures.solver.residual; statsStream << time << ", " << exec_time << ", " << num_contacts / write_steps << ", " << numIters << ", " << residual << ", " << max_cnstr_viol[0] << ", " << max_cnstr_viol[1] << ", \n"; statsStream.GetFstream().flush(); num_contacts = 0; max_cnstr_viol[0] = 0; max_cnstr_viol[1] = 0; } if (problem == PRESSING || problem == TESTING) { // Get the current reaction force or impose load plate position double cnstr_force = 0; if (use_actuator) { cnstr_force = actuator->Get_react_force().x; } else { double zpos_new = pos_old.z + desiredVelocity * time_step; loadPlate->SetPos(ChVector<>(pos_old.x, pos_old.y, zpos_new)); loadPlate->SetPos_dt(ChVector<>(0, 0, desiredVelocity)); } if (sim_frame % write_steps == 0) { // std::cout << time << ", " << loadPlate->GetPos().z << ", " << cnstr_force << ", \n"; sinkageStream << time << ", " << loadPlate->GetPos().z << ", " << cnstr_force << ", \n"; sinkageStream.GetFstream().flush(); } } // Find maximum constraint violation if (!prismatic.IsNull()) { ChMatrix<>* C = prismatic->GetC(); for (int i = 0; i < 5; i++) max_cnstr_viol[0] = std::max(max_cnstr_viol[0], std::abs(C->GetElement(i, 0))); } if (!actuator.IsNull()) { ChMatrix<>* C = actuator->GetC(); max_cnstr_viol[1] = std::max(max_cnstr_viol[2], std::abs(C->GetElement(0, 0))); } // Increment counters time += time_step; sim_frame++; exec_time += msystem->GetTimerStep(); num_contacts += msystem->GetNcontacts(); // If requested, output detailed timing information for this step if (sim_frame == timing_frame) msystem->PrintStepStats(); } // ---------------- // Final processing // ---------------- // Create a checkpoint from the last state if (problem == SETTLING || problem == PRESSING) { cout << " Write checkpoint data " << flush; if (problem == SETTLING) utils::WriteCheckpoint(msystem, settled_ckpnt_file); else utils::WriteCheckpoint(msystem, pressed_ckpnt_file); cout << msystem->Get_bodylist()->size() << " bodies" << endl; } // Final stats cout << "==================================" << endl; cout << "Number of bodies: " << msystem->Get_bodylist()->size() << endl; cout << "Simulation time: " << exec_time << endl; cout << "Number of threads: " << threads << endl; return 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; }
void ChLcpSystemDescriptor::ShurComplementProduct( ChMatrix<>& result, ChMatrix<>* lvector, std::vector<bool>* enabled ) { #ifdef CH_DEBUG assert(this->vstiffness.size() == 0); // currently, the case with ChLcpKblock items is not supported (only diagonal M is supported, no K) int n_c=CountActiveConstraints(); assert(lvector->GetRows() == n_c); assert(lvector->GetColumns()== 1); if (enabled) assert(enabled->size() == n_c); #endif result.Reset(n_c,1); // fast! Reset() method does not realloc if size doesn't change // Performs the sparse product result = [N]*l = [ [Cq][M^(-1)][Cq'] - [E] ] *l // in different phases: // 1 - set the qb vector (aka speeds, in each ChLcpVariable sparse data) as zero #pragma omp parallel for num_threads(this->num_threads) for (int iv = 0; iv< (int)vvariables.size(); iv++) { if (vvariables[iv]->IsActive()) vvariables[iv]->Get_qb().FillElem(0); } // 2 - performs qb=[M^(-1)][Cq']*l by // iterating over all constraints (when implemented in parallel this // could be non-trivial because race conditions might occur -> reduction buffer etc.) // Also, begin to add the cfm term ( -[E]*l ) to the result. //#pragma omp parallel for num_threads(this->num_threads) ***NOT POSSIBLE!!! concurrent write to same q may happen for (int ic = 0; ic < (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { int s_c = vconstraints[ic]->GetOffset(); bool process=true; if (enabled) if ((*enabled)[s_c]==false) process = false; if (process) { double li; if (lvector) li = (*lvector)(s_c,0); else li = vconstraints[ic]->Get_l_i(); // Compute qb += [M^(-1)][Cq']*l_i // NOTE! parallel update to same q data, risk of collision if parallel!! vconstraints[ic]->Increment_q(li); // <----!!! fpu intensive // Add constraint force mixing term result = cfm * l_i = -[E]*l_i result(s_c,0) = vconstraints[ic]->Get_cfm_i() * li; } } } // 3 - performs result=[Cq']*qb by // iterating over all constraints #pragma omp parallel for num_threads(this->num_threads) for (int ic = 0; ic < (int)vconstraints.size(); ic++) { if (vconstraints[ic]->IsActive()) { bool process=true; if (enabled) if ((*enabled)[vconstraints[ic]->GetOffset()]==false) process = false; if (process) result(vconstraints[ic]->GetOffset(),0)+= vconstraints[ic]->Compute_Cq_q(); // <----!!! fpu intensive else result(vconstraints[ic]->GetOffset(),0)= 0; // not enabled constraints, just set to 0 result } } }