virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override { const Vec3 frcColors[] = {Red,Orange,Cyan,Green,Blue,Yellow}; m_system.realize(state, Stage::Velocity); const int ncont = m_compliant.getNumContactForces(state); for (int i=0; i < ncont; ++i) { const ContactForce& force = m_compliant.getContactForce(state,i); const ContactId id = force.getContactId(); // define if it's femur_lat -> femur_med contact pair: id=8 //if (id == 6 || id == 8 || id==4 || id==2 || id==10 ) // continue; ContactSnapshot cs = m_compliant.getContactTrackerSubsystem().getActiveContacts(state); //cout << "contact: " << id << endl; //ContactId idFemur = cs.getContactIdForSurfacePair( ContactSurfaceIndex(2), ContactSurfaceIndex(3)); //ContactId idMeniscFemur1 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(2)); //ContactId idMeniscTibia1 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(4)); //ContactId idMeniscFemur2 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(3)); //ContactId idMeniscTibia2 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(4)); ContactId idLatFemurTibia = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(2)); ContactId idMedFemurTibia = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(2)); ContactId idFemur = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(1)); if (id == idFemur) continue; const SimbodyMatterSubsystem& matter = m_compliant.getMultibodySystem().getMatterSubsystem(); // get tibia's mobilized body MobilizedBody tibiaMobod = matter.getMobilizedBody(MobilizedBodyIndex(22)); MobilizedBody femurLatmobod = matter.getMobilizedBody(MobilizedBodyIndex(19)); MobilizedBody femurMedmobod = matter.getMobilizedBody(MobilizedBodyIndex(20)); //for (int numContacts=0; numContacts<cs.getNumContacts(); numContacts++) //{ //ContactSurfaceIndex csi1 = cs.getContact(numContacts).getSurface1(); //ContactSurfaceIndex csi2 = cs.getContact(numContacts).getSurface2(); //cout << "surf1: " << csi1 << " surf2: " << csi2 << endl; //} if (cs.getNumContacts()>0) { DecorativeGeometry decTibiaContactGeometry = tibiaMobod.getBody().updDecoration(0); decTibiaContactGeometry.setOpacity( 0.9); decTibiaContactGeometry.setColor( Gray); DecorativeGeometry decFemurLatContactGeometry = femurLatmobod.getBody().updDecoration(0); decFemurLatContactGeometry.setOpacity(1); decFemurLatContactGeometry.setColor(Gray); DecorativeGeometry decFemurMedContactGeometry = femurMedmobod.getBody().updDecoration(0); decFemurMedContactGeometry.setOpacity(1); decFemurMedContactGeometry.setColor(Gray); // Tibia transformation const Transform& X_BM = tibiaMobod.getOutboardFrame(state); // M frame in B const Transform& X_GB = tibiaMobod.getBodyTransform(state); // B in Ground const Transform& X_PF = tibiaMobod.getInboardFrame(state); Transform X_GM = X_GB*X_BM; // M frame in Ground // rotate //Rotation& newRot = X_GM.updR(); //Rotation parentRot = X_PF.R(); //newRot.operator*=( parentRot.invert()); // translate in front of the whole body X_GM.setP( X_GM.operator+=( Vec3(0.3,0,0)).p()); // set new transform decTibiaContactGeometry.setTransform( X_GM); // Medial Femur Transformation const Transform& X_BM_medFemur = femurMedmobod.getOutboardFrame(state); // M frame in B const Transform& X_GB_medFemur = femurMedmobod.getBodyTransform(state); // B in Ground const Transform& X_PF_medFemur = femurMedmobod.getInboardFrame(state); Transform X_GM_medFemur = X_GB_medFemur*X_BM_medFemur; // M frame in Ground // rotate //Rotation& newRot_medFemur = X_GM_medFemur.updR(); //Rotation parentRot_medFemur = X_PF_medFemur.R(); //newRot_medFemur.operator*=(parentRot_medFemur).operator*=(Rotation(1.57079, CoordinateAxis::ZCoordinateAxis())); //newRot_medFemur.setRotationFromAngleAboutZ(1.570796326794897); // translate in front of the whole body X_GM_medFemur.setP(X_GM_medFemur.operator+=(Vec3(0.3, 0.1, 0)).p()); // set new transform decFemurMedContactGeometry.setTransform(X_GM_medFemur); // Lateral Femur Transformation const Transform& X_BM_latFemur = femurLatmobod.getOutboardFrame(state); // M frame in B const Transform& X_GB_latFemur = femurLatmobod.getBodyTransform(state); // B in Ground const Transform& X_PF_latFemur = femurLatmobod.getInboardFrame(state); Transform X_GM_latFemur = X_GB_latFemur*X_BM_latFemur; // M frame in Ground // rotate //Rotation& newRot_latFemur = X_GM_latFemur.updR(); //Rotation parentRot_latFemur = X_PF_latFemur.R(); //newRot_latFemur.operator*=(parentRot_latFemur).operator*=(Rotation(1.57079, CoordinateAxis::ZCoordinateAxis())); //newRot_latFemur.setRotationFromAngleAboutZ(1.570796326794897); // translate in front of the whole body X_GM_latFemur.setP(X_GM_latFemur.operator+=(Vec3(0.3, 0.1, 0)).p()); // set new transform decFemurLatContactGeometry.setTransform(X_GM_latFemur); ContactPatch patch; const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch); //cout << "patch for id" << id << " found=" << found << endl; //cout << "resultant=" << patch.getContactForce() << endl; //cout << "num details=" << patch.getNumDetails() << endl; for (int k=0; k < patch.getNumDetails(); ++k) { const ContactDetail& detail = patch.getContactDetail(k); //const Real peakPressure = detail.getPeakPressure(); const Vec3 cp = detail.getContactPoint(); Vec3 newcp = Vec3(cp); // transform point to attach tibia surface Transform cpToTibiaTransf = Transform(newcp); cpToTibiaTransf.setP(cpToTibiaTransf.operator+=( Vec3(0.3,0,0)).p()); DecorativeSphere decCpToTibia; decCpToTibia.setScale(0.0005); decCpToTibia.setTransform(cpToTibiaTransf); decCpToTibia.setColor(Red); // transform point to attach femur surface Transform cpToFemurTransf = Transform(newcp); //Rotation& newRotCpToFemur = cpToFemurTransf.updR(); //newRotCpToFemur.setRotationFromAngleAboutZ(1.570796326794897); //cpToFemurTransf.set(X_GM_latFemur.R(), X_GM_latFemur.p()); //newRotCpToFemur.operator*=(parentRot_latFemur); //cpToFemurTransf.setP( X_PF_latFemur.p()); //newRotCpToFemur.setRotationFromAngleAboutZ(1.570796326794897); cpToFemurTransf.setP(cpToFemurTransf.operator+=(Vec3(0.3, 0.1, 0)).p()); DecorativeSphere decCpToFemur; decCpToFemur.setScale(0.0005); decCpToFemur.setTransform(cpToFemurTransf); decCpToFemur.setColor(Red); geometry.push_back(decCpToTibia); geometry.push_back(decCpToFemur); if (k == 0) { geometry.push_back(decTibiaContactGeometry); geometry.push_back(decFemurLatContactGeometry); geometry.push_back(decFemurMedContactGeometry); } } } } }
ContactGeometry::TriangleMesh::Impl::Impl (const PolygonalMesh& mesh, bool smooth) : ContactGeometryImpl(), smooth(smooth) { // Create the mesh, triangulating faces as necessary. Array_<Vec3> vertexPositions; Array_<int> faceIndices; for (int i = 0; i < mesh.getNumVertices(); i++) vertexPositions.push_back(mesh.getVertexPosition(i)); for (int i = 0; i < mesh.getNumFaces(); i++) { int numVert = mesh.getNumVerticesForFace(i); if (numVert < 3) continue; // Ignore it. if (numVert == 3) { faceIndices.push_back(mesh.getFaceVertex(i, 0)); faceIndices.push_back(mesh.getFaceVertex(i, 1)); faceIndices.push_back(mesh.getFaceVertex(i, 2)); } else if (numVert == 4) { // Split it into two triangles. faceIndices.push_back(mesh.getFaceVertex(i, 0)); faceIndices.push_back(mesh.getFaceVertex(i, 1)); faceIndices.push_back(mesh.getFaceVertex(i, 2)); faceIndices.push_back(mesh.getFaceVertex(i, 2)); faceIndices.push_back(mesh.getFaceVertex(i, 3)); faceIndices.push_back(mesh.getFaceVertex(i, 0)); } else { // Add a vertex at the center, then split it into triangles. Vec3 center(0); for (int j = 0; j < numVert; j++) center += vertexPositions[mesh.getFaceVertex(i, j)]; center /= numVert; vertexPositions.push_back(center); int newIndex = vertexPositions.size()-1; for (int j = 0; j < numVert-1; j++) { faceIndices.push_back(mesh.getFaceVertex(i, j)); faceIndices.push_back(mesh.getFaceVertex(i, j+1)); faceIndices.push_back(newIndex); } // Close the face (thanks, Alexandra Zobova). faceIndices.push_back(mesh.getFaceVertex(i, numVert-1)); faceIndices.push_back(mesh.getFaceVertex(i, 0)); faceIndices.push_back(newIndex); } } init(vertexPositions, faceIndices); // Make sure the mesh normals are oriented correctly. Vec3 origin(0); for (int i = 0; i < 3; i++) origin += vertices[faces[0].vertices[i]].pos; origin /= 3; // this is the face centroid const UnitVec3 direction = -faces[0].normal; // Calculate a ray origin that is guaranteed to be outside the // mesh. If the topology is right (face 0 normal points outward), we'll be // outside on the side containing face 0. If it is wrong, we'll be outside // on the opposite side of the mesh. Then we'll shoot a ray back along the // direction we came from (that is, towards the interior of the mesh from // outside). We'll hit *some* face. If the topology is right, the hit // face's normal will be pointing back at us. If it is wrong, the face // normal will also be pointing inwards, in roughly the same direction as // the ray. origin -= max(obb.bounds.getSize())*direction; Real distance; int face; Vec2 uv; bool intersects = intersectsRay(origin, direction, distance, face, uv); assert(intersects); // Now dot the hit face normal with the ray direction; correct topology // will have them pointing in more-or-less opposite directions. if (dot(faces[face].normal, direction) > 0) { // We need to invert the mesh topology. for (int i = 0; i < (int) faces.size(); i++) { Face& f = faces[i]; int temp = f.vertices[0]; f.vertices[0] = f.vertices[1]; f.vertices[1] = temp; temp = f.edges[1]; f.edges[1] = f.edges[2]; f.edges[2] = temp; f.normal *= -1; } for (int i = 0; i < (int) vertices.size(); i++) vertices[i].normal *= -1; } }
int main() { try { // Currently, this only tests a small number of operations that were recently added. // It should be expanded into a more comprehensive test of the big matrix classes. // Test matrix elementwise initialization. Matrix minit(2,3, 5.25); testMatrix<Matrix,2,3>(minit, Mat23(5.25, 5.25, 5.25, 5.25, 5.25, 5.25)); const Vec2 v12(1,2); Matrix_<Vec2> mvinit(3,2, v12); testMatrix<Matrix_<Vec2>,3,2>(mvinit, Mat<3,2,Vec2>(v12,v12, v12,v12, v12,v12)); testMatDivision(); testTransform(); Matrix m(Mat22(1, 2, 3, 4)); testMatrix<Matrix,2,2>(m, Mat22(1, 2, 3, 4)); m += 3; testMatrix<Matrix,2,2>(m, Mat22(4, 2, 3, 7)); m -= 3; testMatrix<Matrix,2,2>(m, Mat22(1, 2, 3, 4)); testMatrix<Matrix,2,2>(m-1, Mat22(0, 2, 3, 3)); testMatrix<Matrix,2,2>(m+1, Mat22(2, 2, 3, 5)); testMatrix<Matrix,2,2>(1-m, Mat22(0, -2, -3, -3)); testMatrix<Matrix,2,2>(1+m, Mat22(2, 2, 3, 5)); Vector v(Vec3(1, 2, 3)); testVector(v, Vec3(1, 2, 3)); v += 2; testVector(v, Vec3(3, 4, 5)); v -= 2; testVector(v, Vec3(1, 2, 3)); testVector(v-1, Vec3(0, 1, 2)); testVector(v+1, Vec3(2, 3, 4)); testVector(1-v, Vec3(0, -1, -2)); testVector(1+v, Vec3(2, 3, 4)); RowVector r(Row3(1, 2, 3)); testVector(r, Vec3(1, 2, 3)); r += 2; testVector(r, Vec3(3, 4, 5)); r -= 2; testVector(r, Vec3(1, 2, 3)); testVector(r-1, Vec3(0, 1, 2)); testVector(r+1, Vec3(2, 3, 4)); testVector(1-r, Vec3(0, -1, -2)); testVector(1+r, Vec3(2, 3, 4)); Matrix mm( Mat23( 1, 2, 3, 7, 8, 9 ) ); testMatrix<Matrix,2,3>(mm, Mat23(1,2,3,7,8,9)); // Test copying a column or row of a Matrix into // a Vector or RowVector. // Test assignment constructor Vector vv = mm(1); testVector(vv, Vec2(2,8)); // Test copy assignment vv = mm(0); testVector(vv, Vec2(1,7)); // Test assignment constructor RowVector rr = mm[1]; testVector(rr, Vec3(7,8,9)); // Test copy assignment rr = mm[0]; testVector(rr, Vec3(1,2,3)); // Test copying a row into a Vector and column into RowVector. // Test assignment (copy) constructor RowVector rrr = ~mm(1); testVector(rrr, Vec2(2,8)); // Test copy assignment rrr = ~mm(0); testVector(rrr, Vec2(1,7)); // Test assignment (copy) constructor Vector vvv = ~mm[1]; testVector(vvv, Vec3(7,8,9)); // Test copy assignment vvv = ~mm[0]; testVector(vvv, Vec3(1,2,3)); // Test creating a Matrix that shares space with an Array // Easy case: sizeof(element) == sizeof(scalar) Array_<Real> rarrmat; rarrmat.push_back(1.1); rarrmat.push_back(2.2); // col(0) rarrmat.push_back(3.3); rarrmat.push_back(4.4); // col(1) Matrix rmatrix(2,2, 2/*lda*/, &rarrmat[0]); testMatrix<Matrix,2,2>(rmatrix, Mat22(1.1, 3.3, 2.2, 4.4)); // Here sizeof(element) != sizeof(scalar) Array_<SpatialVec> svarrmat; svarrmat.push_back(SpatialVec(Vec3(1,2,3),Vec3(4,5,6))); svarrmat.push_back(SpatialVec(Vec3(1.1,2.1,3.1),Vec3(4.1,5.1,6.1))); svarrmat.push_back(SpatialVec(Vec3(1.2,2.2,3.2),Vec3(4.2,5.2,6.2))); svarrmat.push_back(SpatialVec(Vec3(1.3,2.3,3.3),Vec3(4.3,5.3,6.3))); const int szInScalars = sizeof(SpatialVec)/sizeof(Real); Matrix_<SpatialVec> svmatrix(2,2, 2*szInScalars/*lda*/, (Real*)&svarrmat[0]); Matrix_<SpatialVec> svmatans(2,2); svmatans(0,0) = svarrmat[0]; svmatans(1,0)=svarrmat[1]; svmatans(0,1) = svarrmat[2]; svmatans(1,1)=svarrmat[3]; SimTK_TEST_EQ_TOL(svmatrix, svmatans, 1e-16); // should be exact // Test creating a Vector that shares space with an Array // Easy case: sizeof(element) == sizeof(scalar) Array_<Real> rarray; rarray.push_back(1.1); rarray.push_back(2.2); rarray.push_back(3.3); Vector rvector(3, &rarray[0], true); testVector(rarray, Vec3(1.1,2.2,3.3)); // Here sizeof(element) != sizeof(scalar) Array_<SpatialVec> svarray; svarray.push_back(SpatialVec(Vec3(1,2,3),Vec3(4,5,6))); svarray.push_back(SpatialVec(Vec3(1.1,2.1,3.1),Vec3(4.1,5.1,6.1))); svarray.push_back(SpatialVec(Vec3(1.2,2.2,3.2),Vec3(4.2,5.2,6.2))); Vector_<SpatialVec> svvector(3, (Real*)&svarray[0], true); Vector_<SpatialVec> svanswer(3); svanswer[0]=svarray[0];svanswer[1]=svarray[1];svanswer[2]=svarray[2]; SimTK_TEST_EQ_TOL(svvector, svanswer, 1e-16); // should be exact // Create 0-width slices of Matrix that has general shape, // vector shape, and row vector shape. This caused trouble before // because vector and row shapes use 1d matrix storage; when making // a 0-width slice of those they have to go back to general shape. // Note that you are allowed to index off the bottom and right if // you make a zero-width slice. Matrix general(3, 4); MatrixView gslice1 = general(1,1,0,2); // middle SimTK_TEST(gslice1.nrow()==0 && gslice1.ncol()==2); MatrixView gslice2 = general(1,1,1,0); // middle SimTK_TEST(gslice2.nrow()==1 && gslice2.ncol()==0); MatrixView gslice3 = general(0,0,3,0); // left side SimTK_TEST(gslice3.nrow()==3 && gslice3.ncol()==0); MatrixView gslice4 = general(0,0,0,4); // top SimTK_TEST(gslice4.nrow()==0 && gslice4.ncol()==4); MatrixView gslice5 = general(3,0,0,4); // off the bottom SimTK_TEST(gslice5.nrow()==0 && gslice5.ncol()==4); MatrixView gslice6 = general(0,4,3,0); // off the right side SimTK_TEST(gslice6.nrow()==3 && gslice6.ncol()==0); MatrixView gslice7 = general(0,0,0,0); SimTK_TEST(gslice7.nrow()==0 && gslice7.ncol()==0); MatrixView gslice8 = general(1,2,0,0); SimTK_TEST(gslice8.nrow()==0 && gslice8.ncol()==0); MatrixView gslice9 = general(2,3,0,0); SimTK_TEST(gslice9.nrow()==0 && gslice9.ncol()==0); MatrixView vector = general(0,1,3,1); SimTK_TEST(vector.nrow()==3 && vector.ncol()==1); MatrixView vslice1 = vector(0,0,3,0); SimTK_TEST(vslice1.nrow()==3 && vslice1.ncol()==0); MatrixView vslice2 = vector(0,0,0,0); SimTK_TEST(vslice2.nrow()==0 && vslice2.ncol()==0); MatrixView vslice3 = vector(2,0,1,0); SimTK_TEST(vslice3.nrow()==1 && vslice3.ncol()==0); MatrixView vslice4 = vector(3,0,0,1); // off the bottom SimTK_TEST(vslice4.nrow()==0 && vslice4.ncol()==1); MatrixView vslice5 = vector(0,1,3,0); // off the right SimTK_TEST(vslice5.nrow()==3 && vslice5.ncol()==0); vslice5 = Matrix(3,0); } catch(const std::exception& e) { cout << "exception: " << e.what() << endl; return 1; } cout << "Done" << endl; return 0; }
// Call this on a newly-constructed ModelVisualizer (typically from the Model's // initSystem() method) to set up the various auxiliary classes used for // visualization and user interaction. This involves modifications to the // System that must be done prior to realizeTopology(), and may modify the // Model also. void ModelVisualizer::createVisualizer() { _model.updMatterSubsystem().setShowDefaultGeometry(false); // Allocate a Simbody Visualizer. If environment variable // OPENSIM_HOME is set, add its bin subdirectory to the search path // for the SimbodyVisualizer executable. The search will go as // follows: first look in the same directory as the currently- // executing executable; then look in the $OPENSIM_HOME/bin // directory, then look in various default Simbody places. Array_<String> searchPath; if (SimTK::Pathname::environmentVariableExists("OPENSIM_HOME")) { searchPath.push_back( SimTK::Pathname::getEnvironmentVariable("OPENSIM_HOME") + "/bin"); } _viz = new SimTK::Visualizer(_model.getMultibodySystem(), searchPath); // Make the Simbody Visualizer (that is, the display window) kill itself // when the API-side connection is lost (because the Visualizer object gets // destructed). Otherwise it will hang around afterwards. _viz->setShutdownWhenDestructed(true); _viz->setCameraClippingPlanes(.01,100.); _viz->setBackgroundColor(SimTK::Black); _viz->setBackgroundType(SimTK::Visualizer::SolidColor); // Give it an OpenSim-friendly window heading. bool isAbsolutePath; string directory, fileName, extension; SimTK::Pathname::deconstructPathname( SimTK::Pathname::getThisExecutablePath(), isAbsolutePath, directory, fileName, extension); _viz->setWindowTitle("OpenSim " + OpenSim::GetVersion() + ": " + fileName + " (" + _model.getName() + ")"); // Create a menu for choosing what to display. SimTK::Array_< std::pair<SimTK::String, int> > selections; selections.push_back(std::make_pair("Wrap geometry", ToggleWrapGeometry)); selections.push_back(std::make_pair("Contact geometry", ToggleContactGeometry)); selections.push_back(std::make_pair("Muscle paths",ToggleMusclePaths)); selections.push_back(std::make_pair("Path points",TogglePathPoints)); selections.push_back(std::make_pair("Markers",ToggleMarkers)); selections.push_back(std::make_pair("Frames",ToggleFrames)); selections.push_back(std::make_pair("Default geometry", ToggleDefaultGeometry)); _viz->addMenu("Show", ShowMenuId, selections); // Add a DecorationGenerator to dispatch runtime generateDecorations() // calls. _decoGen = new DefaultGeometry(_model); _viz->addDecorationGenerator(_decoGen); // Add an input listener to handle display menu picks. _viz->addInputListener(new OpenSimInputListener(_model)); // Allocate an InputSilo to pick up anything the above listener doesn't. _silo = new SimTK::Visualizer::InputSilo(); _viz->addInputListener(_silo); // This is used for regular output of frames during forward dynamics. // TODO: allow user control of timing. _model.updMultibodySystem().addEventReporter (new SimTK::Visualizer::Reporter(*_viz, 1./30)); }
VisualizerProtocol::VisualizerProtocol (Visualizer& visualizer, const Array_<String>& userSearchPath) { // Launch the GUI application. We'll first look for one in the same // directory as the running executable; then if that doesn't work we'll // look in the bin subdirectory of the SimTK installation. String vizExecutableName; if (Pathname::environmentVariableExists("SIMBODY_VISUALIZER_NAME")) { vizExecutableName = Pathname::getEnvironmentVariable("SIMBODY_VISUALIZER_NAME"); } else { vizExecutableName = "simbody-visualizer"; #ifndef NDEBUG vizExecutableName += "_d"; #endif } Array_<String> actualSearchPath; // Always start with the current executable's directory. actualSearchPath.push_back(Pathname::getThisExecutableDirectory()); // User's stuff comes next, if any directories were provided. We're going // to turn these into absolute pathnames, interpreting them as defined // by Pathname, which includes executable-relative names. The "bin" // subdirectory if any must already be present in the directory names. for (unsigned i=0; i < userSearchPath.size(); ++i) actualSearchPath.push_back (Pathname::getAbsoluteDirectoryPathname(userSearchPath[i])); if (Pathname::environmentVariableExists("SIMBODY_HOME")) { const std::string e = Pathname::getAbsoluteDirectoryPathname( Pathname::getEnvironmentVariable("SIMBODY_HOME")); actualSearchPath.push_back(Pathname::addDirectoryOffset(e, SIMBODY_VISUALIZER_REL_INSTALL_DIR)); } else if (Pathname::environmentVariableExists("SimTK_INSTALL_DIR")) { const std::string e = Pathname::getAbsoluteDirectoryPathname( Pathname::getEnvironmentVariable("SimTK_INSTALL_DIR")); actualSearchPath.push_back(Pathname::addDirectoryOffset(e, SIMBODY_VISUALIZER_REL_INSTALL_DIR)); } // Try using the location of SimTKsimbody combined with the path from the // SimTKsimbody library to the simbody-visualizer install location (only // available on non-Windows platforms). We must provide a function that // resides in SimTKsimbody. std::string SimTKsimbodyDir; if (Pathname::getFunctionLibraryDirectory((void*)createPipeSim2Viz, SimTKsimbodyDir)) { // We have the path to SimTKsimbody; now we combine this with the path // from SimTKsimbody to simbody-visualizer (this assumes the // installation has not been reorganized from what CMake originally // specified). std::string absPathToVizDir = Pathname::getAbsoluteDirectoryPathnameUsingSpecifiedWorkingDirectory( SimTKsimbodyDir, SIMBODY_PATH_FROM_LIBDIR_TO_VIZ_DIR); actualSearchPath.push_back(absPathToVizDir); } // Try the build-time install location: actualSearchPath.push_back(SIMBODY_VISUALIZER_INSTALL_DIR); // Our last desperate attempts will // be <platformDefaultInstallDir>/Simbody/bin // and <platformDefaultInstallDir>/SimTK/bin const std::string def = Pathname::getDefaultInstallDir(); actualSearchPath.push_back( Pathname::addDirectoryOffset(def, Pathname::addDirectoryOffset("Simbody", SIMBODY_VISUALIZER_REL_INSTALL_DIR))); actualSearchPath.push_back( Pathname::addDirectoryOffset(def, Pathname::addDirectoryOffset("SimTK", SIMBODY_VISUALIZER_REL_INSTALL_DIR))); // Pipe[0] is the read end, Pipe[1] is the write end. int sim2vizPipe[2], viz2simPipe[2], status; // Create pipe pair for communication from simulator to visualizer. status = createPipeSim2Viz(sim2vizPipe); SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe"); outPipe = sim2vizPipe[1]; // write here to talk to visualizer // Create pipe pair for communication from visualizer to simulator. status = createPipeViz2Sim(viz2simPipe); SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe"); inPipe = viz2simPipe[0]; // read from here to receive from visualizer // Spawn the visualizer gui, trying local first then installed version. spawnViz(actualSearchPath, vizExecutableName, sim2vizPipe, viz2simPipe); // Before we do anything else, attempt to exchange handshake messages with // the visualizer. This will throw an exception if anything goes wrong. // Note that this is done on the main thread. shakeHandsWithGUI(outPipe, inPipe); // Spawn the thread to listen for events. pthread_mutex_init(&sceneLock, NULL); pthread_create(&eventListenerThread, NULL, listenForVisualizerEvents, &visualizer); }
VisualizerProtocol::VisualizerProtocol (Visualizer& visualizer, const Array_<String>& userSearchPath) { // Launch the GUI application. We'll first look for one in the same directory // as the running executable; then if that doesn't work we'll look in the // bin subdirectory of the SimTK installation. const char* GuiAppName = "simbody-visualizer"; Array_<String> actualSearchPath; // Always start with the current executable's directory. actualSearchPath.push_back(Pathname::getThisExecutableDirectory()); // User's stuff comes next, if any directories were provided. We're going // to turn these into absolute pathnames, interpreting them as defined // by Pathname, which includes executable-relative names. The "bin" // subdirectory if any must already be present in the directory names. for (unsigned i=0; i < userSearchPath.size(); ++i) actualSearchPath.push_back (Pathname::getAbsoluteDirectoryPathname(userSearchPath[i])); if (Pathname::environmentVariableExists("SIMBODY_HOME")) { const std::string e = Pathname::getAbsoluteDirectoryPathname( Pathname::getEnvironmentVariable("SIMBODY_HOME")); actualSearchPath.push_back(Pathname::addDirectoryOffset(e,"bin")); } else if (Pathname::environmentVariableExists("SimTK_INSTALL_DIR")) { const std::string e = Pathname::getAbsoluteDirectoryPathname( Pathname::getEnvironmentVariable("SimTK_INSTALL_DIR")); actualSearchPath.push_back(Pathname::addDirectoryOffset(e,"bin")); } // Try the build-time install location: actualSearchPath.push_back(SIMBODY_VISUALIZER_INSTALL_DIR); // Our last desperate attempts will // be <platformDefaultInstallDir>/Simbody/bin // and <platformDefaultInstallDir>/SimTK/bin const std::string def = Pathname::getDefaultInstallDir(); actualSearchPath.push_back( Pathname::addDirectoryOffset(def, Pathname::addDirectoryOffset("Simbody", "bin"))); actualSearchPath.push_back( Pathname::addDirectoryOffset(def, Pathname::addDirectoryOffset("SimTK", "bin"))); // Pipe[0] is the read end, Pipe[1] is the write end. int sim2vizPipe[2], viz2simPipe[2], status; // Create pipe pair for communication from simulator to visualizer. status = createPipeSim2Viz(sim2vizPipe); SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe"); outPipe = sim2vizPipe[1]; // write here to talk to visualizer // Create pipe pair for communication from visualizer to simulator. status = createPipeViz2Sim(viz2simPipe); SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe"); inPipe = viz2simPipe[0]; // read from here to receive from visualizer // Spawn the visualizer gui, trying local first then installed version. spawnViz(actualSearchPath, GuiAppName, sim2vizPipe, viz2simPipe); // Before we do anything else, attempt to exchange handshake messages with // the visualizer. This will throw an exception if anything goes wrong. // Note that this is done on the main thread. shakeHandsWithGUI(outPipe, inPipe); // Spawn the thread to listen for events. pthread_mutex_init(&sceneLock, NULL); pthread_t thread; pthread_create(&thread, NULL, listenForVisualizerEvents, &visualizer); }
int constraintJacobian(const Vector& parameters, bool new_parameters, Matrix& J) const override { ++nEvalJacobian; if (new_parameters) setInternalStateFromFreeQs(parameters); for (unsigned i=0; i < assembler.reporters.size(); ++i) assembler.reporters[i]->handleEvent(getInternalState()); assert(J.nrow() == getNumEqualityConstraints()); assert(J.ncol() == getNumFreeQs()); const int n = getNumFreeQs(); // This will record the indices of any constraints we encounter that // can't provide their own gradients; we'll handle them all together // at the end. Array_<AssemblyConditionIndex> needNumericalJacobian; Array_<int> firstEqn; Array_<int> nEqns; int needy = 0; int nxtEqn = 0; for (unsigned i=0; i < assembler.errors.size(); ++i) { AssemblyConditionIndex consIx = assembler.errors[i]; const AssemblyCondition& cond = *assembler.conditions[consIx]; const int m = cond.getNumErrors(getInternalState()); const int stat = (assembler.forceNumericalJacobian ? -1 : cond.calcErrorJacobian(getInternalState(), J(nxtEqn,0,m,n))); if (stat == -1) { needNumericalJacobian.push_back(consIx); firstEqn.push_back(nxtEqn); nEqns.push_back(m); needy += m; } else if (stat != 0) return stat; nxtEqn += m; } if (!needNumericalJacobian.empty()) { //cout << "Need numerical Jacobian for " // << needNumericalJacobian.size() << " constraints." << endl; NumJacobianFunc numCons(assembler, needNumericalJacobian, nEqns, needy); // Forward difference should be fine here, unlike for the // gradient because we converge on the solution value // rather than the derivative norm. Differentiator jacNumCons(numCons); Matrix numJ = jacNumCons.calcJacobian(getFreeQsFromInternalState()); nEvalConstraints += jacNumCons.getNumCallsToUserFunction(); // Fill in the missing rows. int nxtInNumJ = 0; for (unsigned i=0; i < needNumericalJacobian.size(); ++i) { J(firstEqn[i],0,nEqns[i],n) = numJ(nxtInNumJ,0,nEqns[i],n); nxtInNumJ += nEqns[i]; } } //cout << "J=" << J; return 0; }
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) { const Vec3 frcColors[] = {Red,Orange,Cyan}; const Vec3 momColors[] = {Blue,Green,Purple}; m_mbs.realize(state, Stage::Velocity); const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem(); const Real TextScale = m_mbs.getDefaultLengthScale()/10; // was .1 m_mbs.realize(state, Stage::Dynamics); const Real KE=m_mbs.calcKineticEnergy(state), E=m_mbs.calcEnergy(state); const Real diss=m_compliant.getDissipatedEnergy(state); DecorativeText txt; txt.setIsScreenText(true); txt.setText("KE/Diss/E: " + String(KE, "%.6f") + String(diss, "/%.6f") + String(E+diss, "/%.6f") ); geometry.push_back(txt); int nContPts = 0; const int ncont = m_compliant.getNumContactForces(state); for (int i=0; i < ncont; ++i) { const ContactForce& force = m_compliant.getContactForce(state,i); const ContactId id = force.getContactId(); const Vec3& pt = force.getContactPoint(); const Vec3& frc = force.getForceOnSurface2()[1]; const Vec3& mom = force.getForceOnSurface2()[0]; Real frcMag = frc.norm(), momMag=mom.norm(); const UnitVec3 frcDir(frc/frcMag, true); const UnitVec3 momDir(mom/momMag, true); const Vec3 offs = .1*frcDir; // shift up to clear ground int frcThickness = 2, momThickness = 2; Real frcScale = ForceScale, momScale = ForceScale; while (frcMag > /*10*/1000000) frcThickness++, frcScale /= 10, frcMag /= 10; while (momMag > /*10*/1000000) momThickness++, momScale /= 10, momMag /= 10; geometry.push_back(DecorativePoint(pt) .setScale(5).setColor(Yellow)); DecorativeLine frcLine(pt, pt + std::log10(frcMag)*frcDir); DecorativeLine momLine(pt+offs, pt+offs + std::log10(momMag)*momDir); frcLine.setColor(Black); momLine.setColor(Purple); frcLine.setLineThickness(frcThickness); momLine.setLineThickness(momThickness); geometry.push_back(frcLine); geometry.push_back(momLine); ContactPatch patch; const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch); //cout << "patch for id" << id << " found=" << found << endl; //cout << "resultant=" << patch.getContactForce() << endl; //cout << "num details=" << patch.getNumDetails() << endl; for (int i=0; i < patch.getNumDetails(); ++i) { ++nContPts; const ContactDetail& detail = patch.getContactDetail(i); const Vec3& pt = detail.getContactPoint(); geometry.push_back(DecorativePoint(pt).setColor(Purple)); const Vec3& force = detail.getForceOnSurface2(); const Real forceMag = force.norm(); const UnitVec3 forceDir(force/forceMag, true); DecorativeLine frcLine(pt, pt+std::log10(forceMag)*forceDir); frcLine.setColor(Black); geometry.push_back(frcLine); // Make a red line that extends from the contact // point in the direction of the slip velocity, of length 3*slipvel. DecorativeLine slip(pt,pt+3.*detail.getSlipVelocity()); slip.setColor(Red); geometry.push_back(slip); } } txt.setText(String("Num contact points: ") + String(nContPts)); geometry.push_back(txt); }
int main() { try { // catch errors if any // Create the system, with subsystems for the bodies and some forces. MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); Force::Gravity gravity(forces, matter, -YAxis, 9.8); system.setUseUniformBackground(true); // request no ground & sky // Describe a body with a point mass at (0, -3, 0) and draw a sphere there. Real mass = 3; Vec3 pos(0,-3,0); Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos))); bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5)); // Create the tree of mobilized bodies, reusing the above body description. MobilizedBody::Pin bodyT (matter.Ground(), Vec3(0), bodyInfo, Vec3(0)); MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0), bodyInfo, Vec3(0)); MobilizedBody::Pin rtArm (bodyT, Vec3(2, 0, 0), bodyInfo, Vec3(0,-1,0)); // Add some damping. Force::MobilityLinearDamper damper1(forces, bodyT, MobilizerUIndex(0), 10); Force::MobilityLinearDamper damper2(forces, leftArm, MobilizerUIndex(0), 30); Force::MobilityLinearDamper damper3(forces, rtArm, MobilizerUIndex(0), 10); #ifdef USE_TORQUE_LIMITED_MOTOR MyTorqueLimitedMotor* motorp = new MyTorqueLimitedMotor(rtArm, MobilizerUIndex(0), TorqueGain, MaxTorque); const MyTorqueLimitedMotor& motor = *motorp; Force::Custom(forces, motorp); // takes over ownership #else // Use built-in Steady Motion as a low-budget motor model. //Motion::Steady motor(rtArm, InitialMotorRate); // Use built-in ConstantSpeed constraint as a low-budget motor model. Constraint::ConstantSpeed motor(rtArm, InitialMotorRate); #endif // Add a joint stop to the left arm restricting it to q in [0,Pi/5]. Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0), StopStiffness, InitialDissipation, -Pi/8, // lower stop Pi/8); // upper stop Visualizer viz(system); // Add sliders. viz.addSlider("Motor speed", SliderIdMotorSpeed, -10, 10, InitialMotorRate); viz.addSlider("Dissipation", SliderIdDissipation, 0, 10, InitialDissipation); viz.addSlider("Tach", SliderIdTach, -20, 20, 0); viz.addSlider("Torque", SliderIdTorque, -MaxTorque, MaxTorque, 0); // Add Run menu. Array_<std::pair<String,int> > runMenuItems; runMenuItems.push_back(std::make_pair("Reset", ResetItem)); runMenuItems.push_back(std::make_pair("Quit", QuitItem)); viz.addMenu("Run", MenuIdRun, runMenuItems); Visualizer::InputSilo* userInput = new Visualizer::InputSilo(); viz.addInputListener(userInput); // Initialize the system and state. State initState = system.realizeTopology(); // Simulate forever with a small max step size. Check for user input // in between steps. Note: an alternate way to do this is to let the // integrator take whatever steps it wants but use a TimeStepper to // manage a periodic event handler to poll for user input. Here we're // treating completion of a step as an event. const Real MaxStepSize = 0.01*3; // 10ms const int DrawEveryN = 3/3; // 3 steps = 30ms //RungeKuttaMersonIntegrator integ(system); //RungeKutta2Integrator integ(system); SemiExplicitEuler2Integrator integ(system); //SemiExplicitEulerIntegrator integ(system, .001); integ.setAccuracy(1e-1); //integ.setAccuracy(1e-3); // Don't permit interpolation because we want the state returned after // a step to be modifiable. integ.setAllowInterpolation(false); integ.initialize(initState); int stepsSinceViz = DrawEveryN-1; while (true) { if (++stepsSinceViz % DrawEveryN == 0) { const State& s = integ.getState(); viz.report(s); const Real uActual = rtArm.getOneU(s, MobilizerUIndex(0)); viz.setSliderValue(SliderIdTach, uActual); #ifdef USE_TORQUE_LIMITED_MOTOR viz.setSliderValue(SliderIdTorque, motor.getTorque(s)); #else system.realize(s); // taus are acceleration stage //viz.setSliderValue(SliderIdTorque, // rtArm.getOneTau(s, MobilizerUIndex(0))); viz.setSliderValue(SliderIdTorque, motor.getMultiplier(s)); #endif stepsSinceViz = 0; } // Advance time by MaxStepSize (might take multiple steps to get there). integ.stepBy(MaxStepSize); // Now poll for user input. int whichSlider, whichMenu, whichItem; Real newValue; // Did a slider move? if (userInput->takeSliderMove(whichSlider, newValue)) { State& state = integ.updAdvancedState(); switch(whichSlider) { case SliderIdMotorSpeed: // TODO: momentum balance? //motor.setRate(state, newValue); motor.setSpeed(state, newValue); system.realize(state, Stage::Position); system.prescribeU(state); system.realize(state, Stage::Velocity); system.projectU(state); break; case SliderIdDissipation: stop.setMaterialProperties(state, StopStiffness, newValue); system.realize(state, Stage::Position); break; } } // Was there a menu pick? if (userInput->takeMenuPick(whichMenu, whichItem)) { if (whichItem == QuitItem) break; // done // If Reset, stop the motor and restore default dissipation. // Tell visualizer to update the sliders to match. // Zero out all the q's and u's. if (whichItem == ResetItem) { State& state = integ.updAdvancedState(); //motor.setRate(state, 0); motor.setSpeed(state, 0); viz.setSliderValue(SliderIdMotorSpeed, 0); stop.setMaterialProperties(state, StopStiffness, InitialDissipation); viz.setSliderValue(SliderIdDissipation, InitialDissipation); state.updQ() = 0; // all positions to zero state.updU() = 0; // all velocities to zero system.realize(state, Stage::Position); system.prescribeU(state); system.realize(state, Stage::Velocity); system.projectU(state); } } } const int evals = integ.getNumRealizations(); std::cout << "Done -- simulated " << integ.getTime() << "s with " << integ.getNumStepsTaken() << " steps, avg step=" << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms " << (1000*integ.getTime())/evals << "ms/eval\n"; printf("Used Integrator %s at accuracy %g:\n", integ.getMethodName(), integ.getAccuracyInUse()); printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted()); printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures()); printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections()); } catch (const std::exception& e) { std::cout << "ERROR: " << e.what() << std::endl; return 1; } return 0; }
void handleEvent(const State& state) const { saveEm.push_back(state); }
int main() { try { // Create the system. MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); Force::Gravity gravity(forces, matter, UnitVec3(.1,-1,0), 9.81); ContactTrackerSubsystem tracker(system); CompliantContactSubsystem contactForces(system, tracker); contactForces.setTrackDissipatedEnergy(true); contactForces.setTransitionVelocity(1e-3); const Vec3 hdim(1,2,3); const Real fFac =.15; // to turn off friction const Real fDis = .5; // to turn off dissipation const Real fVis = .1; // to turn off viscous friction const Real fK = .1*1e6; // pascals // Halfspace floor const Rotation R_xdown(-Pi/2,ZAxis); matter.Ground().updBody().addDecoration( Transform(Vec3(0,-.5,0)), DecorativeBrick(Vec3(10,.5,20)).setColor(Green).setOpacity(.1)); matter.Ground().updBody().addContactSurface( Transform(R_xdown, Vec3(0,0,0)), ContactSurface(ContactGeometry::HalfSpace(), ContactMaterial(fK*.1,fDis*.9, fFac*.8,fFac*.7,fVis*10))); const Real brickMass = 10; Body::Rigid brickBody(MassProperties(brickMass, Vec3(0), UnitInertia::brick(hdim))); brickBody.addDecoration(Transform(), DecorativeBrick(hdim).setColor(Cyan).setOpacity(.3)); const int surfx = brickBody.addContactSurface(Transform(), ContactSurface(ContactGeometry::Brick(hdim), ContactMaterial(fK,fDis, fFac*.8,fFac*.7,fVis)) ); //brickBody.addContactSurface(Transform(), // ContactSurface(ContactGeometry::Ellipsoid(hdim), // ContactMaterial(fK*.1,fDis*.9, // .1*fFac*.8,.1*fFac*.7,fVis*1)) // ); const ContactSurface& surf = brickBody.getContactSurface(surfx); const ContactGeometry& cg = surf.getShape(); const ContactGeometry::Brick& cgbrick = ContactGeometry::Brick::getAs(cg); cout << "cgbrick.hdim=" << cgbrick.getHalfLengths() << endl; const Geo::Box& box = cgbrick.getGeoBox(); cout << "box.hdim=" << box.getHalfLengths() << endl; // Vertices for (int i=0; i<8; ++i) { const Vec3 vpos = box.getVertexPos(i); const UnitVec3 vn = box.getVertexNormal(i); brickBody.addDecoration (DecorativePoint(vpos).setColor(Orange)); brickBody.addDecoration (DecorativeText(String(i)).setTransform(vpos).setColor(White) .setScale(.5)); brickBody.addDecoration (DecorativeLine(vpos, vpos + 0.5*vn).setColor(Orange)); printf("vertex %d:\n", i); int e[3],ew[3],f[3],fw[3]; box.getVertexEdges(i,e,ew); box.getVertexFaces(i,f,fw); for (int ex=0; ex<3; ++ex) { int ev[2]; box.getEdgeVertices(e[ex], ev); printf(" e%2d(%d) ev=%d\n", e[ex], ew[ex], ev[ew[ex]]); } for (int fx=0; fx<3; ++fx) { int fv[4]; box.getFaceVertices(f[fx], fv); printf(" f%2d(%d) fv=%d\n", f[fx], fw[fx], fv[fw[fx]]); } } // Edges for (int i=0; i<12; ++i) { const UnitVec3 n = box.getEdgeNormal(i); const UnitVec3 d = box.getEdgeDirection(i); const Vec3 ctr = box.getEdgeCenter(i); const Real len = .75; brickBody.addDecoration (DecorativePoint(ctr).setColor(Green).setScale(2)); brickBody.addDecoration (DecorativeText(String(i)).setTransform(ctr+len*n) .setColor(Green).setScale(.3)); brickBody.addDecoration (DecorativeLine(ctr, ctr + len*n).setColor(Green)); brickBody.addDecoration (DecorativeLine(ctr, ctr + len*d).setColor(Green)); printf("edge %d:\n", i); int f[2],fw[2]; box.getEdgeFaces(i,f,fw); for (int fx=0; fx<2; ++fx) { int fe[4]; box.getFaceEdges(f[fx], fe); printf(" f%2d(%d) fe=%d\n", f[fx], fw[fx], fe[fw[fx]]); } } // Faces for (int i=0; i<6; ++i) { int vertices[4]; box.getFaceVertices(i,vertices); const UnitVec3 n = box.getFaceNormal(i); const Vec3 ctr = box.getFaceCenter(i); brickBody.addDecoration (DecorativePoint(ctr).setColor(Magenta).setScale(3)); brickBody.addDecoration (Transform(Rotation(n,ZAxis,Vec3(0,1,0),YAxis),ctr), DecorativeText(String(i)).setColor(Magenta) .setScale(.75).setFaceCamera(false)); brickBody.addDecoration (DecorativeLine(ctr, ctr + 1.*n).setColor(Magenta)); } MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0,3,0)), brickBody, Transform(Vec3(0))); Visualizer viz(system); viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces)); viz.setShowShadows(true); viz.setShowSimTime(true); viz.setDesiredFrameRate(FrameRate); viz.setShowFrameRate(true); viz.setBackgroundType(Visualizer::SolidColor); viz.setBackgroundColor(White*.9); Visualizer::InputSilo* silo = new Visualizer::InputSilo(); viz.addInputListener(silo); Array_<std::pair<String,int> > runMenuItems; runMenuItems.push_back(std::make_pair("Go", GoItem)); runMenuItems.push_back(std::make_pair("Replay", ReplayItem)); runMenuItems.push_back(std::make_pair("Quit", QuitItem)); viz.addMenu("Run", RunMenuId, runMenuItems); Array_<std::pair<String,int> > helpMenuItems; helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1)); viz.addMenu("Help", HelpMenuId, helpMenuItems); system.addEventReporter(new MyReporter(system,contactForces,ReportInterval)); system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval)); // Check for a Run->Quit menu pick every 1/4 second. system.addEventHandler(new UserInputHandler(*silo, .25)); // Initialize the system and state. system.realizeTopology(); State state = system.getDefaultState(); brick.setQToFitRotation(state, Rotation(SpaceRotationSequence, .1, ZAxis, .05, XAxis)); brick.setUToFitLinearVelocity(state, Vec3(2,0,0)); saveEm.reserve(10000); viz.report(state); printf("Default state\n"); cout << "t=" << state.getTime() << " q=" << brick.getQAsVector(state) << " u=" << brick.getUAsVector(state) << endl; cout << "\nChoose 'Go' from Run menu to simulate:\n"; int menuId, item; do { silo->waitForMenuPick(menuId, item); if (menuId != RunMenuId || item != GoItem) cout << "\aDude ... follow instructions!\n"; } while (menuId != RunMenuId || item != GoItem); // Simulate it. // The system as parameterized is very stiff (mostly due to friction) // and thus runs best with CPodes which is extremely stable for // stiff problems. To get reasonable performance out of the explicit // integrators (like the RKs) you'll have to run at a very loose // accuracy like 0.1, or reduce the friction coefficients and // maybe the stiffnesses. //SemiExplicitEuler2Integrator integ(system); //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton); RungeKuttaMersonIntegrator integ(system); //RungeKutta3Integrator integ(system); //VerletIntegrator integ(system); //integ.setMaximumStepSize(1e-0001); //integ.setAccuracy(1e-3); // minimum for CPodes integ.setAccuracy(1e-5); //integ.setAccuracy(.01); TimeStepper ts(system, integ); ts.initialize(state); double cpuStart = cpuTime(); double realStart = realTime(); ts.stepTo(20.0); const double timeInSec = realTime() - realStart; const int evals = integ.getNumRealizations(); cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " << timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step=" << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) " << (1000*ts.getTime())/evals << "ms/eval\n"; cout << " CPU time was " << cpuTime() - cpuStart << "s\n"; printf("Using Integrator %s at accuracy %g:\n", integ.getMethodName(), integ.getAccuracyInUse()); printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted()); printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures()); printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections()); viz.dumpStats(std::cout); // Add as slider to control playback speed. viz.addSlider("Speed", 1, 0, 4, 1); viz.setMode(Visualizer::PassThrough); silo->clear(); // forget earlier input double speed = 1; // will change if slider moves while(true) { cout << "Choose Run/Replay to see that again ...\n"; int menuId, item; silo->waitForMenuPick(menuId, item); if (menuId != RunMenuId) { cout << "\aUse the Run menu!\n"; continue; } if (item == QuitItem) break; if (item != ReplayItem) { cout << "\aHuh? Try again.\n"; continue; } for (double i=0; i < (int)saveEm.size(); i += speed ) { int slider; Real newValue; if (silo->takeSliderMove(slider,newValue)) { speed = newValue; } viz.report(saveEm[(int)i]); } } } catch (const std::exception& e) { std::printf("EXCEPTION THROWN: %s\n", e.what()); exit(1); } catch (...) { std::printf("UNKNOWN EXCEPTION THROWN\n"); exit(1); } return 0; }
void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override { const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem(); const Real TextScale = m_mbs.getDefaultLengthScale()/10; // was .1 m_mbs.realize(state, Stage::Dynamics); const Real KE=m_mbs.calcKineticEnergy(state), E=m_mbs.calcEnergy(state); DecorativeText energy; energy.setIsScreenText(true); energy.setText("Energy/KE: " + String(E, "%.6f") + String(KE, "/%.6f")); geometry.push_back(energy); //cout << "brick q=" << m_brick.getQAsVector(state) << endl; //cout << "brick u=" << m_brick.getUAsVector(state) << endl; m_mbs.realize(state, Stage::Acceleration); for (unsigned i=0; i < m_balls.size(); ++i) { const Vec3 f_GC = m_balls[i].findForceOnSphereInG(state); const Vec3 p_GC = m_balls[i].findContactPointInG(state); geometry.push_back( DecorativeLine(p_GC - f_GC, p_GC).setColor(Red)); DecorativeText sep; sep.setIsScreenText(true); sep.setText(String(i) + ": " + String(m_balls[i].findSeparation(state), "%.6f")); geometry.push_back(sep); sep.setText(" : " + String(m_balls[i].getVelocityErrors(state))); geometry.push_back(sep); sep.setText(" : " + String(m_balls[i].getAccelerationErrors(state))); geometry.push_back(sep); } for (unsigned i=0; i < m_sphsph.size(); ++i) { const Vec3 f_GC = m_sphsph[i].findForceOnSphereBInG(state); const Transform X_GC = m_sphsph[i].findContactFrameInG(state); geometry.push_back( DecorativeFrame().setTransform(X_GC).setColor(Purple)); geometry.push_back( DecorativeLine(X_GC.p() - f_GC, X_GC.p()).setColor(Red)); DecorativeText sep; sep.setIsScreenText(true); sep.setText(String(i) + ": " + String(m_sphsph[i].findSeparation(state), "%.6f")); geometry.push_back(sep); sep.setText(" : " + String(m_sphsph[i].getVelocityErrors(state))); geometry.push_back(sep); sep.setText(" : " + String(m_sphsph[i].getAccelerationErrors(state))); geometry.push_back(sep); //const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem(); //State s2 = state; //m_mbs.realize(s2,Stage::Acceleration); //Vector udot=s2.getUDot(); //Vec3 aerr(0); //const Real du = 1e-6; //for (int u=0; u < matter.getNumMobilities(); ++u) { // s2.updU()[u] += du; m_mbs.realize(s2,Stage::Velocity); // Vec3 verrp = m_sphsph[i].getVelocityErrors(s2); // s2.updU()[u] -= 2*du; m_mbs.realize(s2,Stage::Velocity); // Vec3 verrm = m_sphsph[i].getVelocityErrors(s2); // aerr += ((verrp-verrm) / (2*du)) * udot[u]; // s2.updU()[u]=state.getU()[u]; //} //printf("aerr =%.15g %.15g\n", aerr[0], aerr[1]); //m_mbs.realize(s2, Stage::Acceleration); } for (unsigned i=0; i < m_rods.size(); ++i) { const Constraint::Rod& rod = m_rods[i]; const Real t = rod.getRodTension(state); const UnitVec3 d = rod.findRodOrientationInG(state); // p1->p2 const Vec3 f1_G = t*d; // force on p1 const Vec3 f2_G = -f1_G; // force on p2 const Vec3 p2 = rod.getPointOnBody2(state); const Vec3 p2_G = rod.getMobilizedBody2(). findStationLocationInGround(state, p2); geometry.push_back( DecorativeLine(p2_G - f2_G, p2_G) .setColor(Red).setLineThickness(5)); } }
int main() { // Define the system. MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); Force::Gravity gravity(forces, matter, -YAxis, 9.8/10); //Force::GlobalDamper damp(forces, matter, 1); // Describe mass and visualization properties for a generic body. Real mass = 2; Vec3 hdim(1,.5,.25); Body::Rigid bodyInfo(MassProperties(mass, Vec3(0), UnitInertia::brick(hdim))); bodyInfo.addDecoration(Transform(), DecorativeBrick(hdim).setColor(Orange).setOpacity(.3)); Real pmass = .1; Vec3 phdim(5,.5,2); Body::Rigid platformBody(MassProperties(10*mass, Vec3(0), UnitInertia::ellipsoid(phdim))); platformBody.addDecoration(Transform(), DecorativeEllipsoid(phdim).setColor(Cyan).setOpacity(.1) .setResolution(5)); MobilizedBody::Ball platform(matter.Ground(), Vec3(0), platformBody, phdim/2); //MobilizedBody platform = matter.Ground(); // Create the moving (mobilized) bodies of the pendulum. //MobilizedBody::Free brick(platform, Transform(Vec3(0)), // bodyInfo, Transform(Vec3(0))); MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0)), bodyInfo, Transform(Vec3(0))); Array_<Constraint::SphereOnPlaneContact> balls; Array_<Constraint::SphereOnSphereContact> sphsph; Array_<Constraint::Rod> rods; Rotation ZtoY(-Pi/2, XAxis); //Constraint::PointInPlaneWithStiction pt1(platform, // Transform(ZtoY, Vec3(0,1,0)), // brick, hdim); //pt1.setPlaneDisplayHalfWidth(5); //Constraint::SphereOnPlaneContact ball1(platform, // Transform(ZtoY, Vec3(0,1,0)), // brick, hdim, 0.5, false); //ball1.setPlaneDisplayHalfWidth(5); //balls.push_back(ball1); //Constraint::SphereOnPlaneContact ball2(brick, // Transform(Vec3(0,0,-hdim[2])), // platform, -phdim/2, 0.5, false); //ball2.setPlaneDisplayHalfWidth(5); //balls.push_back(ball2); //Constraint::SphereOnPlaneContact ball3(brick, // Transform(Vec3(0,0,-hdim[2])), // platform, Vec3(-2,3,-.5), .7, true); //ball3.setPlaneDisplayHalfWidth(5); //balls.push_back(ball3); //MobilizedBody::Free ball(matter.Ground(), Vec3(0), // MassProperties(1,Vec3(0),UnitInertia(1,1,1)), // Vec3(0)); //Constraint::SphereOnSphereContact ss(platform, Vec3(-2,1,-.5), .7, // ball, Vec3(0), 1.2, true); //Constraint::SphereOnSphereContact bb(brick, hdim, 0.5, // ball, Vec3(0), 1.2, true); //sphsph.push_back(bb); Constraint::SphereOnSphereContact ss(brick, hdim, 0.5, platform, Vec3(-3,1,-.5), 1.2, false); sphsph.push_back(ss); //Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7, // brick, hdim, 0.5, false); //Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7, // brick, hdim, 0.5, false); //Constraint::SphereOnSphereContact ss(brick, hdim, 0.5, // matter.Ground(), Vec3(-2,3,-.5), .7,true); Constraint::Rod rod1(brick, Vec3(0,hdim[1],hdim[2]), platform, Vec3(0,3,-.5), 1.5*1.2); // Spring to keep the brick near 000. //Force::TwoPointLinearSpring(forces, platform, Vec3(0), // brick, Vec3(0), 4, 1); // Rod to keep the brick near 000. //Constraint::Rod rod1(platform, Vec3(0,0,2), // brick, -hdim, 3); //rods.push_back(rod1); // Try edge/edge contact. Constraint::LineOnLineContact ll(platform, Transform(Rotation(UnitVec3(1,1,1), XAxis, UnitVec3(-XAxis), ZAxis), Vec3(1,1,1)), 2, // hlen brick, Transform(Rotation(UnitVec3(ZAxis), XAxis, Vec3(-1,-1,0), ZAxis), Vec3(-hdim[0],-hdim[1],0)), 2, // hlen true); // Set up visualization at 30 fps. Visualizer viz(system); viz.setBackgroundType(Visualizer::SolidColor); viz.setShowFrameRate(true); system.addEventReporter(new Visualizer::Reporter(viz, 1./30)); // Initialize the system and acquire default state. State state = system.realizeTopology(); brick.setQToFitTransform(state, Vec3(0,5,0)); brick.setUToFitAngularVelocity(state, Vec3(10,10,10)); //rod1.setRodLength(state, 5); viz.report(state); printf("Initial config. Ready to assemble.\n"); getchar(); Assembler asmb(system); asmb.assemble(state); viz.report(state); printf("Assembled. Ready to initialize.\n"); getchar(); //printf("Changed ball3 from rad=%g to rad=%g\n", // ball3.getSphereRadius(state), 1.5); //ball3.setSphereRadius(state, 1.5); //viz.report(state); getchar(); //asmb.assemble(state); //viz.report(state); //printf("Re-assembled. Ready to simulate.\n"); getchar(); // Choose integrator and simulate for 10 seconds. RungeKuttaMersonIntegrator integ(system); //RungeKutta3Integrator integ(system); integ.setAccuracy(1e-8); //integ.setConstraintTolerance(1e-3); TimeStepper ts(system, integ); ts.initialize(state); viz.report(ts.getState()); printf("Initialized. Ready to simulate.\n"); getchar(); viz.addDecorationGenerator(new ShowEnergy(system,brick,balls,sphsph,rods)); ts.stepTo(100.0); printf("# steps=%d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted()); }
// We assume a path name structure like this: // (1) Everything up to and including the final directory separator // character is the directory; the rest is the file name. On return // we fix the slashes in the directory name to suit the current // system ('\' for Windows, '/' otherwise). // (2) If the file name contains a ".", characters after the last // "." are the extension and the last "." is removed. // (3) What's left is the fileName. // We accept both "/" and "\" as separator characters. We leave the // case as it was supplied. // Leading and trailing white space is removed; embedded white space // remains. // Leading "X:" for some drive letter X is recognized on Windows as // a drive specification, otherwise the drive is the current drive. // That is then removed for further processing. // Absolute paths are designated like this: // Leading "/" means root relative (on the drive). // Leading "./" means current working directory relative (on the drive). // Leading "../" is interpreted as "./..". // Leading "@/" means relative to executable location. // Above leading characters are removed and replaced with the // full path name they represent, then the entire path is divided // into components. If a component is "." or "" (empty) it is // removed. If a component is ".." it and the previous component // if any are removed. (".." as a first component will report as // ill formed.) // If there is something ill-formed about the file name we'll return // false. void Pathname::deconstructPathname( const string& pathname, bool& dontApplySearchPath, string& directory, string& fileName, string& extension) { dontApplySearchPath = false; directory.erase(); fileName.erase(); extension.erase(); // Remove all the white space and make all the slashes be forward ones. // (For Windows they'll be changed to backslashes later.) String processed = String::trimWhiteSpace(pathname) .replaceAllChar('\\', '/'); if (processed.empty()) return; // pathname consisted only of white space string drive; removeDriveInPlace(processed, drive); // Now the drive if any has been removed and we're looking at // the beginning of the pathname. // If the pathname in its entirety is just one of these, append // a slash to avoid special cases below. if (processed == "." || processed == ".." || processed == "@") processed += "/"; // If the path begins with "../" we'll make it ./../ to simplify handling. if (processed.substr(0, 3) == "../") processed.insert(0, "./"); if (processed.substr(0, 1) == "/") { dontApplySearchPath = true; processed.erase(0, 1); if (drive.empty()) drive = getCurrentDriveLetter(); } else if (processed.substr(0, 2) == "./") { dontApplySearchPath = true; processed.replace(0, 2, getCurrentWorkingDirectory(drive)); removeDriveInPlace(processed, drive); } else if (processed.substr(0, 2) == "@/") { dontApplySearchPath = true; processed.replace(0, 2, getThisExecutableDirectory()); removeDriveInPlace(processed, drive); } else if (!drive.empty()) { // Looks like a relative pathname. But if it had an initial // drive specification, e.g. X:something.txt, that is supposed // to be interpreted relative to the current working directory // on drive X, just as though it were X:./something.txt. dontApplySearchPath = true; processed.insert(0, getCurrentWorkingDirectory(drive)); removeDriveInPlace(processed, drive); } // We may have picked up a new batch of backslashes above. processed.replaceAllChar('\\', '/'); // Now we have the full pathname if this is absolute, otherwise // we're looking at a relative pathname. In any case the last // component is the file name if it isn't empty, ".", or "..". // Process the ".." segments and eliminate meaningless ones // as we go through. Array_<string> segmentsInReverse; bool isFinalSegment = true; // first time around might be the fileName int numDotDotsSeen = 0; while (!processed.empty()) { string component; removeLastPathComponentInPlace(processed, component); if (component == "..") ++numDotDotsSeen; else if (!component.empty() && component != ".") { if (numDotDotsSeen) --numDotDotsSeen; // skip component else if (isFinalSegment) fileName = component; else segmentsInReverse.push_back(component); } isFinalSegment = false; } // Now we can put together the canonicalized directory. if (dontApplySearchPath) { if (!drive.empty()) directory = drive + ":"; directory += "/"; } for (int i = (int)segmentsInReverse.size() - 1; i >= 0; --i) directory += segmentsInReverse[i] + "/"; // Fix the slashes. makeNativeSlashesInPlace(directory); // If there is a .extension, strip it off. string::size_type lastDot = fileName.rfind('.'); if (lastDot != string::npos) { extension = fileName.substr(lastDot); fileName.erase(lastDot); } }
//------------------------- PROCESS EXPANSION PHASE ---------------------------- bool ContactOn:: processExpansionPhase(MyElementSubset& proximal, State& s) const { SimTK_DEBUG("Entering processExpansionPhase() ...\n"); // Generate an expansion impulse if there were any active contacts that // still have some restitution remaining. Vector expansionImpulse; bool anyChange = false; for (unsigned i=0; i<proximal.m_contact.size(); ++i) { const int which = proximal.m_contact[i]; MyContactElement& uni = m_unis.updContactElement(which); if (uni.isDisabled(s)||uni.isRestitutionDone() ||uni.getEffectiveCoefRest()==0 ||uni.getCompressionImpulse()<=0) continue; uni.setMyExpansionImpulse(s, uni.getEffectiveCoefRest(), expansionImpulse); uni.recordImpulse(MyContactElement::Expansion,s,expansionImpulse); uni.setRestitutionDone(true); anyChange = true; } if (!anyChange) { SimTK_DEBUG("... no expansion impulse -- done.\n"); return false; } // We generated an expansion impulse. Apply it and update velocities. updateVelocities(Vector(), expansionImpulse, s); // Release any constraint that now has a positive velocity. Array_<int> toDisable; for (unsigned i=0; i < proximal.m_contact.size(); ++i) { const int which = proximal.m_contact[i]; const MyContactElement& uni = m_unis.getContactElement(which); if (!uni.isDisabled(s) && uni.getVerr(s) > 0) toDisable.push_back(which); } // Now do the actual disabling (can't mix this with checking velocities) // because disabling invalidates Instance stage. for (unsigned i=0; i < toDisable.size(); ++i) { const int which = toDisable[i]; const MyContactElement& uni = m_unis.getContactElement(which); uni.disable(s); } SimTK_DEBUG(" Expansion results:\n"); m_mbs.realize(s, Stage::Velocity); for (unsigned i=0; i < proximal.m_contact.size(); ++i) { const int which = proximal.m_contact[i]; const MyContactElement& uni = m_unis.getContactElement(which); SimTK_DEBUG4(" %d %3s: Ie=%g, V=%g\n", which, uni.isDisabled(s) ? "off" : "ON", uni.getExpansionImpulse(), uni.getVerr(s)); } SimTK_DEBUG("... expansion phase done.\n"); return true; }
void DefaultGeometry::generateDecorations (const State& state, Array_<SimTK::DecorativeGeometry>& geometry) { const SimbodyMatterSubsystem& matter = _model.getMatterSubsystem(); const ModelDisplayHints& hints = _model.getDisplayHints(); // Display wrap objects. if (hints.get_show_wrap_geometry()) { const Vec3 color(SimTK::Cyan); Transform ztoy; ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2); const BodySet& bodies = _model.getBodySet(); for (int i = 0; i < bodies.getSize(); i++) { const OpenSim::Body& body = bodies[i]; const Transform& X_GB = body.getMobilizedBody().getBodyTransform(state); const WrapObjectSet& wrapObjects = body.getWrapObjectSet(); for (int j = 0; j < wrapObjects.getSize(); j++) { const string type = wrapObjects[j].getConcreteClassName(); if (type == "WrapCylinder") { const WrapCylinder* cylinder = dynamic_cast<const WrapCylinder*>(&wrapObjects[j]); if (cylinder != NULL) { Transform X_GW = X_GB*cylinder->getTransform()*ztoy; geometry.push_back( DecorativeCylinder(cylinder->getRadius(), cylinder->getLength()/2) .setTransform(X_GW).setResolution(_dispWrapResolution) .setColor(color).setOpacity(_dispWrapOpacity)); } } else if (type == "WrapEllipsoid") { const WrapEllipsoid* ellipsoid = dynamic_cast<const WrapEllipsoid*>(&wrapObjects[j]); if (ellipsoid != NULL) { Transform X_GW = X_GB*ellipsoid->getTransform(); geometry.push_back( DecorativeEllipsoid(ellipsoid->getRadii()) .setTransform(X_GW).setResolution(_dispWrapResolution) .setColor(color).setOpacity(_dispWrapOpacity)); } } else if (type == "WrapSphere") { const WrapSphere* sphere = dynamic_cast<const WrapSphere*>(&wrapObjects[j]); if (sphere != NULL) { Transform X_GW = X_GB*sphere->getTransform(); geometry.push_back( DecorativeSphere(sphere->getRadius()) .setTransform(X_GW).setResolution(_dispWrapResolution) .setColor(color).setOpacity(_dispWrapOpacity)); } } } } } // Display contact geometry objects. if (hints.get_show_contact_geometry()) { const Vec3 color(SimTK::Green); Transform ztoy; ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2); const ContactGeometrySet& contactGeometries = _model.getContactGeometrySet(); for (int i = 0; i < contactGeometries.getSize(); i++) { const PhysicalFrame& body = contactGeometries.get(i).getBody(); const Transform& X_GB = matter.getMobilizedBody(body.getMobilizedBodyIndex()).getBodyTransform(state); const string type = contactGeometries.get(i).getConcreteClassName(); const int displayPref = contactGeometries.get(i).getDisplayPreference(); //cout << type << ": " << contactGeometries.get(i).getName() << ": disp pref = " << displayPref << endl; if (type == "ContactSphere" && displayPref == 4) { ContactSphere* sphere = dynamic_cast<ContactSphere*>(&contactGeometries.get(i)); if (sphere != NULL) { Transform X_GW = X_GB*sphere->getTransform(); geometry.push_back( DecorativeSphere(sphere->getRadius()) .setTransform(X_GW).setResolution(_dispContactResolution) .setColor(color).setOpacity(_dispContactOpacity)); } } } } // Ask all the ModelComponents to generate dynamic geometry. _model.generateDecorations(false, _model.getDisplayHints(), state, geometry); }