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