int main(int argc, char **argv) { ros::init(argc, argv, "arm_kinematics"); Kinematics k; if (k.init()<0) { ROS_ERROR("Could not initialize kinematics node"); return -1; } ros::spin(); return 0; }
void PlatformCtrlNode::receiveOdo(const sensor_msgs::JointState& js) { mutex.lock(); //odometry msgs nav_msgs::Odometry odom; odom.header.stamp = ros::Time::now(); odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; kin->execForwKin(js, odom, p); topicPub_Odometry.publish(odom); //odometry transform: if(sendTransform) { geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = odom.header.stamp; odom_trans.header.frame_id = odom.header.frame_id; odom_trans.child_frame_id = odom.child_frame_id; odom_trans.transform.translation.x = odom.pose.pose.position.x; odom_trans.transform.translation.y = odom.pose.pose.position.y; odom_trans.transform.translation.z = odom.pose.pose.position.z; odom_trans.transform.rotation = odom.pose.pose.orientation; odom_broadcaster.sendTransform(odom_trans); } mutex.unlock(); }
void PlatformCtrlNode::receiveCmd(const geometry_msgs::Twist& twist) { mutex.lock(); trajectory_msgs::JointTrajectory traj; kin->execInvKin(twist,traj); topicPub_DriveCommands.publish(traj); mutex.unlock(); }
//**************************************************** // Simple init function //**************************************************** void initScene(){ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); glEnable(GL_DEPTH_TEST); //glEnable(GL_LIGHTING); glLightfv(GL_LIGHT0, GL_AMBIENT, lights[0]); glLightfv(GL_LIGHT0, GL_DIFFUSE, lights[0]); glLightfv(GL_LIGHT0, GL_SPECULAR, lights[0]); GLfloat pos[] = {2000,2000,2000}; glLightfv(GL_LIGHT0, GL_POSITION, pos); glEnable(GL_LIGHT0); glShadeModel(GL_FLAT); object = glGenLists(1); for (int i = 0; i < appendages.size(); i++) { test.solveFK(appendages[i],0, 0.001, 0); } glNewList(object, GL_COMPILE); glLineWidth(1); glBegin(GL_LINES); glColor3f(1, 0, 0); glVertex3f(0,0,0); glVertex3f(100,0,0); glColor3f(0, 1, 0); glVertex3f(0,0,0); glVertex3f(0,100,0); glColor3f(0, 0, 1); glVertex3f(0,0,0); glVertex3f(0,0,100); glEnd(); glBegin(GL_LINE_STRIP); float t = 0.0f; while (t<16) { Eigen::Vector3d temp = coolShapes[shape](t); glVertex3f(temp[0], temp[1], temp[2]); t += 0.01f; } glEnd(); if (octopus) { glPushMatrix(); glScalef(1.0f,1.0f,1.5f); glutSolidSphere(1, 10, 10); glPopMatrix(); } glEndList(); glClearColor(0.0, 0.0, 0.0, 0.0); }
void renderIK() { //Eigen::Vector3d goal = interpolateGoal(); for (int i = 0; i < appendages.size(); i++) { test.solveIK(appendages[i], goals[i]); if (octopus) { glPushMatrix(); glTranslatef(cos((2*PI/goals.size())*i), sin((2*PI/goals.size())*i), 0); } glColor3f(0, 1, 1); renderCylinder_convenient(0, 0, 0, appendages[i][0].currPos[0], appendages[i][0].currPos[1], appendages[i][0].currPos[2], 0.04, 10); glPushMatrix(); glTranslatef(appendages[i][0].currPos[0], appendages[i][0].currPos[1], appendages[i][0].currPos[2]); glutSolidSphere(0.1, 10, 10); glPopMatrix(); for (int j=1; j<appendages[i].size(); j++) { if (j%2==0) { glColor3f(0, 1, 1); } else { glColor3f(1, 0, 1); } renderCylinder_convenient(appendages[i][j-1].currPos[0], appendages[i][j-1].currPos[1], appendages[i][j-1].currPos[2], appendages[i][j].currPos[0], appendages[i][j].currPos[1], appendages[i][j].currPos[2], 0.04, 5); glPushMatrix(); glTranslatef(appendages[i][j].currPos[0], appendages[i][j].currPos[1], appendages[i][j].currPos[2]); glutSolidSphere(0.1, 10, 10); glPopMatrix(); } glBegin(GL_LINES); glColor3f(1, 1, 0); glVertex3f(0,0,0); glVertex3f(goals[i][0],goals[i][1],goals[i][2]); glEnd(); if (octopus) glPopMatrix(); } }
bool TauAnalysisSelector::process(Long64_t entry) { double weight = 1.0; if(!fPuWeightName.empty()) { weight *= fPuWeight->value(); } fEventCounter.setWeight(weight); cAll.increment(); if(fIsEmbedded) { weight *= fGeneratorWeight->value(); fEventCounter.setWeight(weight); } cGeneratorWeight.increment(); /* std::cout << "FOO Event " << fEventInfo.event() << ":" << fEventInfo.lumi() << ":" << fEventInfo.run() << " electronVeto passed? " << fElectronVetoPassed->value() << std::endl; if( (fEventInfo.event() == 10069461 && fEventInfo.lumi() == 33572) || (fEventInfo.event() == 10086951 && fEventInfo.lumi() == 33630) || (fEventInfo.event() == 101065527 && fEventInfo.lumi() == 336953) || (fEventInfo.event() == 101450418 && fEventInfo.lumi() == 338236) || (fEventInfo.event() == 101460111 && fEventInfo.lumi() == 338268) ) { std::cout << "BAR Event " << fEventInfo.event() << ":" << fEventInfo.lumi() << ":" << fEventInfo.run() << " electronVeto passed? " << fElectronVetoPassed->value() << " Nelectrons " << fElectrons.size() << std::endl; for(size_t i=0; i<fElectrons.size(); ++i) { ElectronCollection::Electron electron = fElectrons.get(i); std::cout << " Electron " << i << " pt " << electron.p4().Pt() << " eta " << electron.p4().Eta() << " hasGsfTrack " << electron.hasGsfTrack() << " hasSuperCluster " << electron.hasSuperCluster() << " supercluster eta " << electron.superClusterEta() << " passIdVeto " << electron.cutBasedIdVeto() << std::endl; } } if(!fElectronVetoPassed->value()) return false; cElectronVeto.increment(); */ // Tau BR if(fIsEmbedded && fEmbeddingNormalizationMode == kIDTriggerEfficiencyTauBR) { weight *= (1-0.357); // from my thesis, which took the numbers from PDG fEventCounter.setWeight(weight); } cTauBRWeight.increment(); // MC status std::vector<GenParticleCollection::GenParticle> genTaus; GenParticleCollection::GenParticle theGenTau; if(isMC()) { bool canContinue = true; if(fIsEmbedded) { canContinue = findGenTaus(fGenTausEmbedded, 2212, &genTaus); canContinue = canContinue && genTaus.size() != 0; } else canContinue = findGenTaus(fGenTaus, 6, &genTaus); if(!canContinue) return false; } cGenTauFound.increment(); if(isMC() && fMCTauMultiplicity != kMCTauNone) { size_t ntaus = genTaus.size(); if(fIsEmbedded) { std::vector<GenParticleCollection::GenParticle> genTausOrig; findGenTaus(fGenTaus, 6, &genTausOrig); ntaus = genTausOrig.size(); } if(fIsEmbedded) { // For embedded the embedded tau is counted as one if(fMCTauMultiplicity == kMCTauZeroTaus) return true; if(fMCTauMultiplicity == kMCTauOneTau && ntaus != 0) return true; if(fMCTauMultiplicity == kMCTauTwoTaus && ntaus != 1) return true; if(fMCTauMultiplicity == kMCTauMoreThanTwoTaus && ntaus < 2) return true; } else { if(fMCTauMultiplicity == kMCTauZeroTaus && ntaus != 0) return true; if(fMCTauMultiplicity == kMCTauOneTau && ntaus != 1) return true; if(fMCTauMultiplicity == kMCTauTwoTaus && ntaus != 2) return true; if(fMCTauMultiplicity == kMCTauMoreThanTwoTaus && ntaus < 3) return true; } if(genTaus.size() > 0) theGenTau = genTaus[0]; } cTauMCSelection.increment(); bool originalMuonIsWMu = false; EmbeddingMuonCollection::Muon embeddingMuon; if(fIsEmbedded) { if(fMuons.size() != 1) throw std::runtime_error("Embedding muon collection size is not 1"); embeddingMuon = fMuons.get(0); if(embeddingMuon.p4().Pt() < 41) return true; //std::cout << "Muon pt " << muon.p4().Pt() << std::endl; originalMuonIsWMu = std::abs(embeddingMuon.pdgId()) == 13 && std::abs(embeddingMuon.motherPdgId()) == 24 && std::abs(embeddingMuon.grandMotherPdgId()) == 6; if(!originalMuonIsWMu) return true; } cOnlyWMu.increment(); // Per-event correction for W->tau->mu if(fIsEmbedded && fEmbeddingWTauMuWeights) { embeddingMuon.ensureValidity(); int bin = fEmbeddingWTauMuWeights->FindBin(embeddingMuon.p4().Pt()); //std::cout << embeddingMuon.p4().Pt() << " " << bin << std::endl; weight *= fEmbeddingWTauMuWeights->GetBinContent(bin); fEventCounter.setWeight(weight); } cWTauMuWeight.increment(); // Muon trigger efficiency weighting if(fIsEmbedded && (fEmbeddingNormalizationMode == kIDTriggerEfficiency || fEmbeddingNormalizationMode == kIDTriggerEfficiencyTauBR)) { weight *= 1/embeddingMuon.triggerEfficiency(); fEventCounter.setWeight(weight); } cTriggerEffWeight.increment(); // Muon ID efficiency weighting if(fIsEmbedded) { weight *= 1/embeddingMuon.idEfficiency(); fEventCounter.setWeight(weight); } cIdEffWeight.increment(); // Weighting by arbitrary histogram, given in constructor argument if(fIsEmbedded && fEmbeddingMuonWeights) { embeddingMuon.ensureValidity(); int bin = fEmbeddingMuonWeights->FindFixBin(embeddingMuon.p4().Pt()); weight *= fEmbeddingMuonWeights->GetBinContent(bin); fEventCounter.setWeight(weight); } cMuonWeight.increment(); // Jet selection /* size_t njets = fJets.size(); size_t nselectedjets = 0; for(size_t i=0; i<njets; ++i) { JetCollection::Jet jet = fJets.get(i); // Clean selected muon from jet collection bool matches = false; double DR = ROOT::Math::VectorUtil::DeltaR(fMuons.get(0).p4(), jet.p4()); if(DR < 0.1) { matches = true; } if(matches) continue; // Count jets ++nselectedjets; } if(nselectedjets < 3) return true; */ cJetSelection.increment(); if(fSelectedVertexCount->value() <= 0) return true; cPrimaryVertex.increment(); std::vector<TauCollection::Tau> selectedTaus; std::vector<TauCollection::Tau> tmp; if(fTaus.size() < 1) return true; cAllTauCandidates.increment(); // Pre pT cut (for some reason in the region pT < 10 there is significantly more normal than embedded) for(size_t i=0; i<fTaus.size(); ++i) { TauCollection::Tau tau = fTaus.get(i); if(tau.p4().Pt() < 10) continue; if(!TauID::isolation(tau)) continue; //if(isMC() && !fIsEmbedded && tau.genMatchP4().Pt() <= 41) continue; // temporary hack selectedTaus.push_back(tau); if(!fIsEmbedded) break; // select only the first gen tau } if(selectedTaus.empty()) return true; cPrePtCut.increment(); // Decay mode finding bool atLeastOneIsolated = false; for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::decayModeFinding(tau)) continue; if(TauID::isolation(tau)) { atLeastOneIsolated = true; hTau_AfterDecayModeFindingIsolation.fill(tau.p4(), weight); } tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cDecayModeFinding.increment(); if(atLeastOneIsolated) { if(theGenTau.isValid()) hGenTau_AfterDecayModeFindingIsolation.fill(theGenTau.p4(), weight); hVertexCount_AfterDecayModeFindingIsolation->Fill(fVertexCount->value(), weight); } // Eta cut atLeastOneIsolated = false; for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::eta(tau)) continue; if(TauID::isolation(tau)) { atLeastOneIsolated = true; hTau_AfterEtaCutIsolation.fill(tau.p4(), weight); } tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cEtaCut.increment(); if(atLeastOneIsolated) { hGenTau_AfterEtaCutIsolation.fill(theGenTau.p4(), weight); hVertexCount_AfterEtaCutIsolation->Fill(fVertexCount->value(), weight); } // Pt cut atLeastOneIsolated = false; for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::pt(tau)) continue; if(TauID::isolation(tau)) { atLeastOneIsolated = true; hTau_AfterPtCutIsolation.fill(tau.p4(), weight); hTauLeadingTrackPt_AfterPtCutIsolation->Fill(tau.leadPFChargedHadrCandP4().Pt(), weight); } tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cPtCut.increment(); if(atLeastOneIsolated) { hGenTau_AfterPtCutIsolation.fill(theGenTau.p4(), weight); hVertexCount_AfterPtCutIsolation->Fill(fVertexCount->value(), weight); } // Leading track pt atLeastOneIsolated = false; for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::leadingChargedHadrCandPt(tau)) continue; tmp.push_back(tau); if(TauID::isolation(tau)) { atLeastOneIsolated = true; hTau_AfterLeadingTrackPtCutIsolation.fill(tau.p4(), weight); } } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cLeadingTrackPtCut.increment(); if(atLeastOneIsolated) { hGenTau_AfterLeadingTrackPtCutIsolation.fill(theGenTau.p4(), weight); } // ECAL fiducial cuts for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::ecalCracks(tau)) continue; tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cEcalCracks.increment(); for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::ecalGap(tau)) continue; tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cEcalGap.increment(); // Against electron atLeastOneIsolated = false; for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::againstElectron(tau)) continue; tmp.push_back(tau); if(TauID::isolation(tau)) { atLeastOneIsolated = true; hTau_AfterAgainstElectronIsolation.fill(tau.p4(), weight); } } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cAgainstElectron.increment(); if(atLeastOneIsolated) { hGenTau_AfterAgainstElectronIsolation.fill(theGenTau.p4(), weight); } // Against muon atLeastOneIsolated = false; for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::againstMuon(tau)) continue; tmp.push_back(tau); if(TauID::isolation(tau)) { atLeastOneIsolated = true; hTau_AfterAgainstMuonIsolation.fill(tau.p4(), weight); } } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cAgainstMuon.increment(); if(atLeastOneIsolated) { hGenTau_AfterAgainstMuonIsolation.fill(theGenTau.p4(), weight); } // Tau candidate selection finished // Isolation for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::isolation(tau)) continue; int decayMode = tau.decayMode(); int fill = -1; if (decayMode <= 2) fill = decayMode; // 0 = pi+, 1 = pi+pi0, 2 = pi+pi0pi0 else if(decayMode == 10) fill = 3; // pi+pi-pi+ else fill = 4; // Other hTau_AfterIsolation.fill(tau.p4(), weight); hTauDecayModeAll_AfterIsolation->Fill(decayMode, weight); hTauDecayMode_AfterIsolation->Fill(fill, weight); tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cIsolation.increment(); hVertexCount_AfterIsolation->Fill(fVertexCount->value(), weight); if(theGenTau.isValid()) hGenTau_AfterIsolation.fill(theGenTau.p4(), weight); // One prong for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::oneProng(tau)) continue; tmp.push_back(tau); hTau_AfterOneProng.fill(tau.p4(), weight); hTauP_AfterOneProng->Fill(tau.p4().P(), weight); hTauLeadingTrackPt_AfterOneProng->Fill(tau.leadPFChargedHadrCandP4().Pt(), weight); hTauLeadingTrackP_AfterOneProng->Fill(tau.leadPFChargedHadrCandP4().P(), weight); hTauRtau_AfterOneProng->Fill(tau.rtau(), weight); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cOneProng.increment(); hGenTau_AfterOneProng.fill(theGenTau.p4(), weight); hVertexCount_AfterOneProng->Fill(fVertexCount->value(), weight); // Rtau for(size_t i=0; i<selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; if(!TauID::rtau(tau)) continue; hTau_AfterRtau.fill(tau.p4(), weight); tmp.push_back(tau); } selectedTaus.swap(tmp); tmp.clear(); if(selectedTaus.empty()) return true; cRtau.increment(); hVertexCount_AfterRtau->Fill(fVertexCount->value(), weight); if(theGenTau.isValid()) hGenTau_AfterRtau.fill(theGenTau.p4(), weight); // Tau ID finished // Trigger /* if(fIsEmbedded) { if(!fMuTriggerPassed->value()) return true; } */ cMuTrigger.increment(); if(false) { std::cout << "Event " << fEventInfo.event() << ":" << fEventInfo.lumi() << ":" << fEventInfo.run() << std::endl; for(size_t i=0; i< selectedTaus.size(); ++i) { TauCollection::Tau& tau = selectedTaus[i]; std::cout << " selected tau pt " << tau.p4().Pt() << " eta " << tau.p4().Eta() << " phi " << tau.p4().Phi() << std::endl; if(fIsEmbedded) { embeddingMuon.ensureValidity(); double DR = ROOT::Math::VectorUtil::DeltaR(tau.p4(), embeddingMuon.p4()); if(DR < 0.5) { std::cout << " matched to embedding muon, DR " << DR << std::endl; } } for(size_t j=0; j<fGenTaus.size(); ++j) { GenParticleCollection::GenParticle gen = fGenTaus.get(j); double DR = ROOT::Math::VectorUtil::DeltaR(tau.p4(), gen.p4()); if(DR < 0.5) { std::cout << " matched to generator tau, DR " << DR << " mother " << gen.motherPdgId() << " grandmother " << gen.grandMotherPdgId() << std::endl; } } } } return true; }
void TauAnalysisSelector::setOutput(TDirectory *dir) { if(dir) dir->cd(); hTau_AfterDecayModeFindingIsolation.book("tau_AfterDecayModeFindingIsolation"); hGenTau_AfterDecayModeFindingIsolation.book("gentau_AfterDecayModeFindingIsolation"); hVertexCount_AfterDecayModeFindingIsolation = makeVertexCount("vertexCount_AfterDecayModeFindingIsolation"); hTau_AfterEtaCutIsolation.book("tau_AfterEtaCutIsolation"); hGenTau_AfterEtaCutIsolation.book("gentau_AfterEtaCutIsolation"); hVertexCount_AfterEtaCutIsolation = makeVertexCount("vertexCount_AfterEtaCutIsolation"); hTau_AfterPtCutIsolation.book("tau_AfterPtCutIsolation"); hGenTau_AfterPtCutIsolation.book("gentau_AfterPtCutIsolation"); hTauLeadingTrackPt_AfterPtCutIsolation = makePt("tau_AfterPtCutIsolation_leadingTrackPt"); hVertexCount_AfterPtCutIsolation = makeVertexCount("vertexCount_AfterPtCutIsolation"); hTau_AfterLeadingTrackPtCutIsolation.book("tau_AfterLeadingTrackPtCutIsolation"); hGenTau_AfterLeadingTrackPtCutIsolation.book("gentau_AfterLeadingTrackPtCutIsolation"); hTau_AfterAgainstElectronIsolation.book("tau_AfterAgainstElectronIsolation"); hGenTau_AfterAgainstElectronIsolation.book("gentau_AfterAgainstElectronIsolation"); hTau_AfterAgainstMuonIsolation.book("tau_AfterAgainstMuonIsolation"); hGenTau_AfterAgainstMuonIsolation.book("gentau_AfterAgainstMuonIsolation"); hTau_AfterIsolation.book("tau_AfterIsolation"); hGenTau_AfterIsolation.book("gentau_AfterIsolation"); hTauDecayMode_AfterIsolation = makeTH<TH1F>("tauDecayMode_AfterIsolation", "Decay mode", 5, 0, 5); hTauDecayModeAll_AfterIsolation = makeTH<TH1F>("tauDecayModeAll_AfterIsolation", "Decay mode", 16, 0, 16); hVertexCount_AfterIsolation = makeVertexCount("vertexCount_AfterIsolation"); hTau_AfterOneProng.book("tau_AfterOneProng"); hGenTau_AfterOneProng.book("gentau_AfterOneProng"); hTauP_AfterOneProng = makePt("tau_AfterOneProng_p"); hTauLeadingTrackPt_AfterOneProng = makePt("tau_AfterOneProng_leadingTrackPt"); hTauLeadingTrackP_AfterOneProng = makePt("tau_AfterOneProng_leadingTrackP"); hTauRtau_AfterOneProng = makeTH<TH1F>("tau_AfterOneProng_rtau", "Rtau", 22, 0, 1.1); hVertexCount_AfterOneProng = makeVertexCount("vertexCount_AfterOneProng"); hTau_AfterRtau.book("tau_AfterRtau"); hGenTau_AfterRtau.book("gentau_AfterRtau"); hVertexCount_AfterRtau = makeVertexCount("vertexCount_AfterRtau"); }
void testRollingOnSurfaceConstraint() { using namespace SimTK; cout << endl; cout << "=================================================================" << endl; cout << " OpenSim RollingOnSurfaceConstraint Simulation " << endl; cout << "=================================================================" << endl; // angle of the rot w.r.t. vertical double theta = -SimTK::Pi / 6; // 30 degs double omega = -2.1234567890; double halfRodLength = 1.0 / (omega*omega); UnitVec3 surfaceNormal(0,1,0); double planeHeight = 0.0; Vec3 comInRod(0, halfRodLength, 0); Vec3 contactPointOnRod(0, 0, 0); double mass = 7.0; SimTK::Inertia inertiaAboutCom = mass*SimTK::Inertia::cylinderAlongY(0.1, 1.0); SimTK::MassProperties rodMass(7.0, comInRod, inertiaAboutCom.shiftFromMassCenter(comInRod, mass)); // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Create a free joint between the rod and ground MobilizedBody::Planar rod(matter.Ground(), Transform(Vec3(0)), SimTK::Body::Rigid(rodMass), Transform()); // Get underlying mobilized bodies SimTK::MobilizedBody surface = matter.getGround(); // Add a fictitious massless body to be the "Case" reference body coincident with surface for the no-slip constraint SimTK::MobilizedBody::Weld cb(surface, SimTK::Body::Massless()); // Constrain the rod to move on the ground surface SimTK::Constraint::PointInPlane contactY(surface, surfaceNormal, planeHeight, rod, contactPointOnRod); SimTK::Constraint::ConstantAngle contactTorqueAboutY(surface, SimTK::UnitVec3(1, 0, 0), rod, SimTK::UnitVec3(0, 0, 1)); // Constrain the rod to roll on surface and not slide SimTK::Constraint::NoSlip1D contactPointXdir(cb, SimTK::Vec3(0), SimTK::UnitVec3(1, 0, 0), surface, rod); SimTK::Constraint::NoSlip1D contactPointZdir(cb, SimTK::Vec3(0), SimTK::UnitVec3(0, 0, 1), surface, rod); // Simbody model state setup system.realizeTopology(); State state = system.getDefaultState(); //state = system.realizeTopology(); state.updQ()[0] = theta; state.updQ()[1] = 0; state.updQ()[2] = 0; state.updU()[0] = omega; system.realize(state, Stage::Acceleration); state.getUDot().dump("Simbody Accelerations"); Vec3 pcom = system.getMatterSubsystem().calcSystemMassCenterLocationInGround(state); Vec3 vcom = system.getMatterSubsystem().calcSystemMassCenterVelocityInGround(state); Vec3 acom = system.getMatterSubsystem().calcSystemMassCenterAccelerationInGround(state); //========================================================================================================== // Setup OpenSim model Model *osimModel = new Model; osimModel->setGravity(gravity_vec); //OpenSim bodies Ground& ground = osimModel->updGround();; Mesh arrowGeom("arrow.vtp"); arrowGeom.setColor(Vec3(1, 0, 0)); ground.attachGeometry(arrowGeom.clone()); //OpenSim rod auto osim_rod = new OpenSim::Body("rod", mass, comInRod, inertiaAboutCom); OpenSim::PhysicalOffsetFrame* cylFrame = new PhysicalOffsetFrame(*osim_rod, Transform(comInRod)); cylFrame->setName("comInRod"); osimModel->addFrame(cylFrame); Mesh cylGeom("cylinder.vtp"); cylGeom.set_scale_factors(2 * halfRodLength*Vec3(0.1, 1, 0.1)); cylFrame->attachGeometry(cylGeom.clone()); // create rod as a free joint auto rodJoint = new PlanarJoint("rodToGround", ground, *osim_rod); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(osim_rod); osimModel->addJoint(rodJoint); // add a point on line constraint auto roll = new RollingOnSurfaceConstraint(); roll->setRollingBodyByName("rod"); roll->setSurfaceBodyByName("ground"); /*double h = */roll->get_surface_height(); osimModel->addConstraint(roll); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // Need to setup model before adding an analysis since it creates the AnalysisSet // for the model if it does not exist. //osimModel->setUseVisualizer(true); State osim_state = osimModel->initSystem(); roll->setDisabled(osim_state, false); osim_state.updY() = state.getY(); // compute model accelerations osimModel->computeStateVariableDerivatives(osim_state); osim_state.getUDot().dump("Osim Accelerations"); //osimModel->updVisualizer().updSimbodyVisualizer() // .setBackgroundType(SimTK::Visualizer::GroundAndSky); //osimModel->getVisualizer().show(osim_state); Vec3 osim_pcom = osimModel->calcMassCenterPosition(osim_state); Vec3 osim_vcom = osimModel->calcMassCenterVelocity(osim_state); Vec3 osim_acom = osimModel->calcMassCenterAcceleration(osim_state); Vec3 tol(SimTK::SignificantReal); ASSERT_EQUAL(pcom, osim_pcom, tol); ASSERT_EQUAL(vcom, osim_vcom, tol); ASSERT_EQUAL(acom, osim_acom, tol); //========================================================================================================== // Compare Simbody system and OpenSim model simulations compareSimulations(system, state, osimModel, osim_state, "testRollingOnSurfaceConstraint FAILED\n"); }
void testCoordinateCouplerConstraint() { using namespace SimTK; cout << endl; cout << "=================================================================" << endl; cout << " OpenSim CoordinateCouplerConstraint vs. FunctionBasedMobilizer " << endl; cout << "=================================================================" << endl; // Define spline data for the custom knee joint int npx = 12; double angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197, -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443, 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393}; double kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000, 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000, -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000}; int npy = 7; double angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, -0.174532925199, 0.159148563428, 2.094395102393}; double kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, -0.396600000000, -0.395264000000, -0.396000000000 }; for(int i = 0; i<npy; i++) { // Spline data points from experiment w.r.t. hip location. Change to make it w.r.t knee location kneeY[i] += (-kneeInFemur[1]+hipInFemur[1]); } SimmSpline tx(npx, angX, kneeX); SimmSpline ty(npy, angY, kneeY);; // Define the functions that specify the FunctionBased Mobilized Body. std::vector<std::vector<int> > coordIndices; std::vector<const SimTK::Function*> functions; std::vector<bool> isdof(6,false); // Set the 1 spatial rotation about Z-axis isdof[2] = true; //rot Z int nm = 0; for(int i=0; i<6; i++){ if(isdof[i]) { Vector coeff(2); coeff[0] = 1; coeff[1] = 0; std::vector<int> findex(1); findex[0] = nm++; functions.push_back(new SimTK::Function::Linear(coeff)); coordIndices.push_back(findex); } else if(i==3 || i ==4){ std::vector<int> findex(1,0); if(i==3) functions.push_back(tx.createSimTKFunction()); else functions.push_back(ty.createSimTKFunction()); coordIndices.push_back(findex); } else{ std::vector<int> findex(0); functions.push_back(new SimTK::Function::Constant(0, 0)); coordIndices.push_back(findex); } } // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); //system.updDefaultSubsystem().addEventReporter(new VTKEventReporter(system, 0.01)); // Thigh connected by hip MobilizedBody::Pin thigh(matter.Ground(), Transform(hipInGround), SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur)); //Function-based knee connects shank MobilizedBody::FunctionBased shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia), nm, functions, coordIndices); //MobilizedBody::Pin shank(thigh, SimTK::Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), SimTK::Transform(kneeInTibia)); // Simbody model state setup system.realizeTopology(); State state = system.getDefaultState(); matter.setUseEulerAngles(state, true); system.realizeModel(state); //========================================================================================================== // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies const Ground& ground = osimModel->getGround();; OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM); // create hip as a pin joint PinJoint hip("hip",ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0)); // Rename hip coordinates for a pin joint hip.getCoordinateSet()[0].setName("hip_flex"); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_thigh); osimModel->addJoint(&hip); // Add another body via a knee joint OpenSim::Body osim_shank("shank", tibiaMass.getMass(), tibiaMass.getMassCenter(), tibiaMass.getInertia()); // Define knee coordinates and axes for custom joint spatial transform SpatialTransform kneeTransform; string knee_q_name = "knee_q"; string tx_name = "knee_tx"; string ty_name = "knee_ty"; Array<string> indepCoords(knee_q_name, 1, 1); // knee flexion/extension kneeTransform[2].setCoordinateNames(indepCoords); kneeTransform[2].setFunction(new LinearFunction()); // translation X kneeTransform[3].setCoordinateNames(OpenSim::Array<std::string>(tx_name, 1, 1)); kneeTransform[3].setFunction(new LinearFunction()); // translation Y kneeTransform[4].setCoordinateNames(OpenSim::Array<std::string>(ty_name, 1, 1)); kneeTransform[4].setFunction(new LinearFunction()); // create custom knee joint CustomJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, kneeInTibia, Vec3(0), kneeTransform); // Add the shank body which now also contains the knee joint to the model osimModel->addBody(&osim_shank); osimModel->addJoint(&knee); // Constrain the knee translations to follow the desired manifold CoordinateCouplerConstraint knee_tx_constraint; CoordinateCouplerConstraint knee_ty_constraint; knee_tx_constraint.setName("knee_tx_coupler"); knee_ty_constraint.setName("knee_ty_coupler"); knee_tx_constraint.setIndependentCoordinateNames(indepCoords); knee_ty_constraint.setIndependentCoordinateNames(indepCoords); knee_tx_constraint.setDependentCoordinateName(tx_name); knee_ty_constraint.setDependentCoordinateName(ty_name); knee_tx_constraint.setFunction(tx); knee_ty_constraint.setFunction(ty); // Add the constraints osimModel->addConstraint(&knee_tx_constraint); osimModel->addConstraint(&knee_ty_constraint); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // OpenSim model must realize the topology to get valid osim_state osimModel->setGravity(gravity_vec); PointKinematics *pointKin = new PointKinematics(osimModel); // Get the point location of the shank origin in space pointKin->setBodyPoint("shank", Vec3(0)); osimModel->addAnalysis(pointKin); // Model cannot own model components created on the stack in this test program osimModel->disownAllComponents(); // write out the model to file osimModel->print("testCouplerConstraint.osim"); //wipe-out the model just constructed delete osimModel; // reconstruct from the model file osimModel = new Model("testCouplerConstraint.osim"); ForceReporter *forceReport = new ForceReporter(osimModel); forceReport->includeConstraintForces(true); osimModel->addAnalysis(forceReport); // Need to setup model before adding an analysis since it creates the AnalysisSet // for the model if it does not exist. State& osim_state = osimModel->initSystem(); //========================================================================================================== // Compare Simbody system and OpenSim model simulations compareSimulations(system, state, osimModel, osim_state, "testCoordinateCouplerConstraint FAILED\n"); // Forces were held in storage during simulation, now write to file forceReport->printResults("CouplerModelForces"); }
void testPointOnLineConstraint() { using namespace SimTK; cout << endl; cout << "==================================================================" << endl; cout << "OpenSim PointOnLineConstraint vs. Simbody Constraint::PointOnLine " << endl; cout << "==================================================================" << endl; Random::Uniform randomDirection(-1, 1); Vec3 lineDirection(randomDirection.getValue(), randomDirection.getValue(), randomDirection.getValue()); UnitVec3 normLineDirection(lineDirection.normalize()); Vec3 pointOnLine(0,0,0); Vec3 pointOnFollower(0,0,0); // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Create a free joint between the foot and ground MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)), SimTK::Body::Rigid(footMass), Transform(Vec3(0))); // Constrain foot to line on ground SimTK::Constraint::PointOnLine simtkPointOnLine(matter.Ground(), normLineDirection, pointOnLine, foot, pointOnFollower); // Simbody model state setup system.realizeTopology(); State state = system.getDefaultState(); matter.setUseEulerAngles(state, true); system.realizeModel(state); //========================================================================= // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies const Ground& ground = osimModel->getGround();; //OpenSim foot OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia()); // create foot as a free joint FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_foot); osimModel->addJoint(&footJoint); // add a point on line constraint PointOnLineConstraint lineConstraint(ground, normLineDirection, pointOnLine, osim_foot, pointOnFollower); osimModel->addConstraint(&lineConstraint); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // Need to setup model before adding an analysis since it creates the AnalysisSet // for the model if it does not exist. State& osim_state = osimModel->initSystem(); //========================================================================================================== // Compare Simbody system and OpenSim model simulations compareSimulations(system, state, osimModel, osim_state, "testPointOnLineConstraint FAILED\n"); } // end testPointOnLineConstraint
void testWeldConstraint() { using namespace SimTK; cout << endl; cout << "==================================================================" << endl; cout << " OpenSim WeldConstraint vs. Simbody Constraint::Weld " << endl; cout << "==================================================================" << endl; Random::Uniform randomValue(-0.05, 0.1); Vec3 weldInGround(randomValue.getValue(), randomValue.getValue(), 0); Vec3 weldInFoot(0.1*randomValue.getValue(), 0.1*randomValue.getValue(), 0); // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Thigh connected by hip MobilizedBody::Pin thigh(matter.Ground(), SimTK::Transform(hipInGround), SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur)); // Pin knee connects shank MobilizedBody::Pin shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia)); // Pin ankle connects foot MobilizedBody::Pin foot(shank, Transform(ankleInTibia), SimTK::Body::Rigid(footMass), Transform(ankleInFoot)); SimTK::Constraint::Weld weld(matter.Ground(), Transform(weldInGround), foot, Transform(weldInFoot)); // Simbody model state setup system.realizeTopology(); State state = system.getDefaultState(); matter.setUseEulerAngles(state, true); system.realizeModel(state); //========================================================================================================== // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies const Ground& ground = osimModel->getGround();; //OpenSim thigh OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM); // create Pin hip joint PinJoint hip("hip", ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_thigh); osimModel->addJoint(&hip); //OpenSim shank OpenSim::Body osim_shank("shank", tibiaMass.getMass(), tibiaMass.getMassCenter(), tibiaMass.getInertia()); // create Pin knee joint PinJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), osim_shank, kneeInTibia, Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_shank); osimModel->addJoint(&knee); //OpenSim foot OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia()); // create Pin ankle joint PinJoint ankle("ankle", osim_shank, ankleInTibia, Vec3(0), osim_foot, ankleInFoot, Vec3(0)); // Add the foot body which now also contains the hip joint to the model osimModel->addBody(&osim_foot); osimModel->addJoint(&ankle); // add a point on line constraint WeldConstraint footConstraint( "footConstraint", ground, SimTK::Transform(weldInGround), osim_foot, SimTK::Transform(weldInFoot) ); osimModel->addConstraint(&footConstraint); // BAD: but if model maintains ownership, it will attempt to delete stack allocated objects osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); osimModel->setup(); osimModel->print("testWeldConstraint.osim"); // Need to setup model before adding an analysis since it creates the AnalysisSet // for the model if it does not exist. State& osim_state = osimModel->initSystem(); //========================================================================= // Compare Simbody system and OpenSim model simulations compareSimulations(system, state, osimModel, osim_state, "testWeldConstraint FAILED\n"); }
void testConstantDistanceConstraint() { using namespace SimTK; cout << endl; cout << "==================================================================" << endl; cout << " OpenSim ConstantDistanceConstraint vs. Simbody Constraint::Rod " << endl; cout << "==================================================================" << endl; Random::Uniform randomLocation(-1, 1); Vec3 pointOnFoot(randomLocation.getValue(), randomLocation.getValue(), randomLocation.getValue()); Vec3 pointOnGround(0,0,0); /** for some reason, adding another Random::Uniform causes testWeldConstraint to fail. Why doesn't it cause this test to fail???? */ //Random::Uniform randomLength(0.01, 0.2); //randomLength.setSeed(1024); //double rodLength = randomLength.getValue(); double rodLength = 0.05; //std::cout << "Random Length = " << rodLength2 << ", used length = " << rodLength << std::endl; // Define the Simbody system MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec); // Create a free joint between the foot and ground MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)), SimTK::Body::Rigid(footMass), Transform(Vec3(0))); // Constrain foot to point on ground SimTK::Constraint::Rod simtkRod(matter.Ground(), pointOnGround, foot, pointOnFoot, rodLength); // Simbody model state setup system.realizeTopology(); State state = system.getDefaultState(); matter.setUseEulerAngles(state, true); system.realizeModel(state); //========================================================================================================== // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies const Ground& ground = osimModel->getGround();; //OpenSim foot OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia()); // create foot as a free joint FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0)); // Add the thigh body which now also contains the hip joint to the model osimModel->addBody(&osim_foot); osimModel->addJoint(&footJoint); // add a constant distance constraint ConstantDistanceConstraint rodConstraint(ground, pointOnGround, osim_foot, pointOnFoot,rodLength); osimModel->addConstraint(&rodConstraint); // BAD: have to set memoryOwner to false or program will crash when this test is complete. osimModel->disownAllComponents(); osimModel->setGravity(gravity_vec); //Add analyses before setting up the model for simulation Kinematics *kinAnalysis = new Kinematics(osimModel); kinAnalysis->setInDegrees(false); osimModel->addAnalysis(kinAnalysis); // Need to setup model before adding an analysis since it creates the AnalysisSet // for the model if it does not exist. State& osim_state = osimModel->initSystem(); //========================================================================================================== // Compare Simbody system and OpenSim model simulations compareSimulations(system, state, osimModel, osim_state, "testConstantDistanceConstraint FAILED\n"); }
void testExternalLoad() { using namespace SimTK; Model model("Pendulum.osim"); State &s = model.initSystem(); // Simulate gravity double init_t =-1e-8; double final_t = 2.0; int nsteps = 10; double dt = final_t/nsteps; //initial state double q_init = Pi/4; model.updCoordinateSet()[0].setValue(s, q_init); Vector_<double> q_grav(nsteps+1); // Integrator and integration manager double integ_accuracy = 1e-6; RungeKuttaMersonIntegrator integrator(model.getMultibodySystem() ); integrator.setAccuracy(integ_accuracy); Manager manager(model, integrator); manager.setInitialTime(init_t); for(int i = 0; i < nsteps+1; i++){ manager.setFinalTime(dt*i); manager.integrate(s); q_grav[i] = model.updCoordinateSet()[0].getValue(s); manager.setInitialTime(dt*i); } //q_grav.dump("Coords due to gravity."); /***************************** CASE 1 ************************************/ // Simulate the same system without gravity but with an equivalent external load OpenSim::Body &pendulum = model.getBodySet().get(model.getNumBodies()-1); string pendBodyName = pendulum.getName(); Vec3 comInB = pendulum.getMassCenter(); Storage forceStore; addLoadToStorage(forceStore, pendulum.getMass()*model.getGravity(), comInB, Vec3(0, 0, 0)); forceStore.setName("test_external_loads.sto"); forceStore.print(forceStore.getName()); // Apply external force with force in ground, point in body, zero torque ExternalForce xf(forceStore, "force", "point", "torque", pendBodyName, "ground", pendBodyName); xf.setName("grav"); ExternalLoads* extLoads = new ExternalLoads(model); extLoads->adoptAndAppend(&xf); extLoads->print("ExternalLoads_test.xml"); for(int i=0; i<extLoads->getSize(); i++) model.addForce(&(*extLoads)[i]); // Create the force reporter ForceReporter* reporter = new ForceReporter(); model.addAnalysis(reporter); Kinematics* kin = new Kinematics(); kin->setInDegrees(false); model.addAnalysis(kin); PointKinematics* pKin = new PointKinematics(); pKin->setBody(&pendulum); pKin->setPoint(comInB); pKin->setPointName(pendulum.getName()+"_com_p"); model.addAnalysis(pKin); SimTK::State& s2 = model.initSystem(); // Turn-off gravity in the model model.updGravityForce().disable(s2); // initial position model.updCoordinateSet()[0].setValue(s2, q_init); RungeKuttaMersonIntegrator integrator2(model.getMultibodySystem() ); integrator2.setAccuracy(integ_accuracy); Manager manager2(model, integrator2); manager2.setInitialTime(init_t); // Simulate with the external force applied instead of gravity Vector_<double> q_xf(nsteps+1); Vector_<Vec3> pcom_xf(nsteps+1); for(int i = 0; i < nsteps+1; i++){ manager2.setFinalTime(dt*i); manager2.integrate(s2); q_xf[i] = model.updCoordinateSet()[0].getValue(s2); manager2.setInitialTime(dt*i); } //q_xf.dump("Coords due to external force point expressed in pendulum."); Vector err = q_xf-q_grav; double norm_err = err.norm(); // kinematics should match to within integ accuracy ASSERT_EQUAL(0.0, norm_err, integ_accuracy); /***************************** CASE 2 ************************************/ // Simulate the same system without gravity but with an equivalent external // force but this time with the point expressed in ground and using // previous kinematics to transform point to pendulum. // Construct a new Storage for ExternalForce data source with point // described in ground Storage forceStore2 = reporter->getForceStorage(); forceStore2.print("ForcesTest.sto"); Storage *pStore = pKin->getPositionStorage(); pStore->print("PointInGroundTest.sto"); pStore->addToRdStorage(forceStore2, init_t, final_t); forceStore2.setName("ExternalForcePointInGround.sto"); forceStore2.print(forceStore2.getName()); Storage *qStore = kin->getPositionStorage(); qStore->print("LoadKinematics.sto"); string id_base = pendBodyName+"_"+xf.getName(); string point_id = pKin->getPointName(); ExternalForce xf2(forceStore2, id_base+"_F", point_id, id_base+"_T", pendBodyName, "ground", "ground"); xf2.setName("xf_pInG"); // Empty out existing external forces extLoads->setMemoryOwner(false); extLoads->setSize(0); extLoads->adoptAndAppend(&xf2); //Ask external loads to transform point expressed in ground to the applied body extLoads->setDataFileName(forceStore2.getName()); extLoads->invokeConnectToModel(model); extLoads->transformPointsExpressedInGroundToAppliedBodies(*qStore); // remove previous external force from the model too model.disownAllComponents(); model.updForceSet().setSize(0); // after external loads has transformed the point of the force, then add it the model for(int i=0; i<extLoads->getSize(); i++) model.addForce(&(*extLoads)[i]); // recreate dynamical system to reflect new force SimTK::State &s3 = model.initSystem(); // Turn-off gravity in the model model.updGravityForce().disable(s3); // initial position model.updCoordinateSet()[0].setValue(s3, q_init); RungeKuttaMersonIntegrator integrator3(model.getMultibodySystem() ); integrator3.setAccuracy(integ_accuracy); Manager manager3(model, integrator3); manager3.setInitialTime(init_t); // Simulate with the external force applied instead of gravity Vector_<double> q_xf2(nsteps+1); for(int i = 0; i < nsteps+1; i++){ manager3.setFinalTime(dt*i); manager3.integrate(s3); q_xf2[i] = model.updCoordinateSet()[0].getValue(s3); manager3.setInitialTime(dt*i); } //q_xf2.dump("Coords due to external force point expressed in ground."); err = q_xf2-q_grav; //err.dump("Coordinate error after transforming point."); norm_err = err.norm(); // kinematics should match to within integ accuracy ASSERT_EQUAL(0.0, norm_err, integ_accuracy); }
//========================================================================================================== // Test Cases //========================================================================================================== int testBouncingBall(bool useMesh) { // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity, Vec3(0), Inertia()); osimModel->addBody(&ground); OpenSim::Body ball; ball.setName("ball"); ball.set_mass(mass); ball.set_mass_center(Vec3(0)); ball.setInertia(Inertia(1.0)); // Add joints FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0)); osimModel->addBody(&ball); osimModel->addJoint(&free); // Create ContactGeometry. ContactHalfSpace *floor = new ContactHalfSpace(Vec3(0), Vec3(0, 0, -0.5*SimTK_PI), ground, "ground"); osimModel->addContactGeometry(floor); OpenSim::ContactGeometry* geometry; if (useMesh) geometry = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball"); else geometry = new ContactSphere(radius, Vec3(0), ball, "ball"); osimModel->addContactGeometry(geometry); OpenSim::Force* force; if (useMesh) { // Add an ElasticFoundationForce. OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/radius, 1e-5, 0.0, 0.0, 0.0); contactParams->addGeometry("ball"); contactParams->addGeometry("ground"); force = new OpenSim::ElasticFoundationForce(contactParams); osimModel->addForce(force); } else { // Add a HuntCrossleyForce. OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 1e-5, 0.0, 0.0, 0.0); contactParams->addGeometry("ball"); contactParams->addGeometry("ground"); force = new OpenSim::HuntCrossleyForce(contactParams); osimModel->addForce(force); } osimModel->setGravity(gravity_vec); osimModel->setName("TestContactGeomtery_Ball"); osimModel->clone()->print("TestContactGeomtery_Ball.osim"); Kinematics* kin = new Kinematics(osimModel); osimModel->addAnalysis(kin); SimTK::State& osim_state = osimModel->initSystem(); osim_state.updQ()[4] = height; osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); //Initial system energy is all potential double Etot_orig = mass*(-gravity_vec[1])*height; //========================================================================================================== // Simulate it and see if it bounces correctly. cout << "stateY=" << osim_state.getY() << std::endl; RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() ); integrator.setAccuracy(integ_accuracy); Manager manager(*osimModel, integrator); for (unsigned int i = 0; i < duration/interval; ++i) { manager.setInitialTime(i*interval); manager.setFinalTime((i+1)*interval); manager.integrate(osim_state); double time = osim_state.getTime(); osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration); Vec3 pos, vel; osimModel->updSimbodyEngine().getPosition(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), pos); osimModel->updSimbodyEngine().getVelocity(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), vel); double Etot = mass*((-gravity_vec[1])*pos[1] + 0.5*vel[1]*vel[1]); //cout << "starting system energy = " << Etot_orig << " versus current energy = " << Etot << endl; // contact absorbs and returns energy so make sure not in contact if (pos[1] > 2*radius) { ASSERT_EQUAL(Etot_orig, Etot, 1e-2, __FILE__, __LINE__, "Bouncing ball on plane Failed: energy was not conserved."); } else { cout << "In contact at time = " << time << endl; ASSERT(pos[1] < 5.0 && pos[1] > 0); } ASSERT_EQUAL(0.0, pos[0], 1e-4); ASSERT_EQUAL(0.0, pos[2], 1e-4); ASSERT_EQUAL(0.0, vel[0], 1e-3); ASSERT_EQUAL(0.0, vel[2], 1e-3); } std::string prefix = useMesh?"Kinematics_Mesh":"Kinematics_NoMesh"; kin->printResults(prefix); osimModel->disownAllComponents(); // model takes ownership of components unless container set is told otherwise delete osimModel; return 0; }
// test sphere to sphere contact using elastic foundation with and without // meshes and their combination int testBallToBallContact(bool useElasticFoundation, bool useMesh1, bool useMesh2) { // Setup OpenSim model Model *osimModel = new Model; //OpenSim bodies OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity, Vec3(0), Inertia()); osimModel->addBody(&ground); OpenSim::Body ball; ball.setName("ball"); ball.setMass(mass); ball.setMassCenter(Vec3(0)); ball.setInertia(Inertia(1.0)); // Add joints FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0)); osimModel->addBody(&ball); osimModel->addJoint(&free); // Create ContactGeometry. OpenSim::ContactGeometry *ball1, *ball2; if (useElasticFoundation && useMesh1) ball1 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ground, "ball1"); else ball1 = new ContactSphere(radius, Vec3(0), ground, "ball1"); if (useElasticFoundation && useMesh2) ball2 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball2"); else ball2 = new ContactSphere(radius, Vec3(0), ball, "ball2"); osimModel->addContactGeometry(ball1); osimModel->addContactGeometry(ball2); OpenSim::Force* force; std::string prefix; if (useElasticFoundation){ } else{ } if (useElasticFoundation) { // Add an ElasticFoundationForce. OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/(2*radius), 0.001, 0.0, 0.0, 0.0); contactParams->addGeometry("ball1"); contactParams->addGeometry("ball2"); force = new OpenSim::ElasticFoundationForce(contactParams); prefix = "EF_"; prefix += useMesh1 ?"Mesh":"noMesh"; prefix += useMesh2 ? "_to_Mesh":"_to_noMesh"; } else { // Add a Hertz HuntCrossleyForce. OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 0.001, 0.0, 0.0, 0.0); contactParams->addGeometry("ball1"); contactParams->addGeometry("ball2"); force = new OpenSim::HuntCrossleyForce(contactParams); prefix = "Hertz"; } force->setName("contact"); osimModel->addForce(force); osimModel->setGravity(gravity_vec); osimModel->setName(prefix); osimModel->clone()->print(prefix+".osim"); Kinematics* kin = new Kinematics(osimModel); osimModel->addAnalysis(kin); ForceReporter* reporter = new ForceReporter(osimModel); osimModel->addAnalysis(reporter); SimTK::State& osim_state = osimModel->initSystem(); osim_state.updQ()[4] = height; osimModel->getMultibodySystem().realize(osim_state, Stage::Position ); //========================================================================================================== // Simulate it and see if it bounces correctly. cout << "stateY=" << osim_state.getY() << std::endl; RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() ); integrator.setAccuracy(integ_accuracy); integrator.setMaximumStepSize(100*integ_accuracy); Manager manager(*osimModel, integrator); manager.setInitialTime(0.0); manager.setFinalTime(duration); manager.integrate(osim_state); kin->printResults(prefix); reporter->printResults(prefix); osimModel->disownAllComponents(); // model takes ownership of components unless container set is told otherwise delete osimModel; return 0; }