diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index c3bd7f174..1918a9003 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,6 +1,6 @@ SUBDIRS( HelloWorld BasicDemo ) IF(BUILD_BULLET3) - SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow ) + SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint ) ENDIF() IF(BUILD_PYBULLET) diff --git a/examples/TwoJoint/CMakeLists.txt b/examples/TwoJoint/CMakeLists.txt new file mode 100644 index 000000000..4b3259b82 --- /dev/null +++ b/examples/TwoJoint/CMakeLists.txt @@ -0,0 +1,179 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src + ${BULLET_PHYSICS_SOURCE_DIR}/examples + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src + ) + + +SET(RobotSimulator_SRCS + TwoJointMain.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.h + ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp + ../../examples/SharedMemory/InProcessMemory.cpp + ../../examples/SharedMemory/PhysicsClient.cpp + ../../examples/SharedMemory/PhysicsClient.h + ../../examples/SharedMemory/PhysicsServer.cpp + ../../examples/SharedMemory/PhysicsServer.h + ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp + ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.h + ../../examples/SharedMemory/PhysicsDirect.cpp + ../../examples/SharedMemory/PhysicsDirect.h + ../../examples/SharedMemory/PhysicsDirectC_API.cpp + ../../examples/SharedMemory/PhysicsDirectC_API.h + ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp + ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/b3PluginManager.cpp + + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.h + ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h + ../../examples/SharedMemory/PhysicsClientC_API.cpp + ../../examples/SharedMemory/PhysicsClientC_API.h + ../../examples/SharedMemory/Win32SharedMemory.cpp + ../../examples/SharedMemory/Win32SharedMemory.h + ../../examples/SharedMemory/PosixSharedMemory.cpp + ../../examples/SharedMemory/PosixSharedMemory.h + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Utils/b3ResourcePath.h + ../../examples/Utils/RobotLoggingUtil.cpp + ../../examples/Utils/RobotLoggingUtil.h + ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/MultiThreading/b3PosixThreadSupport.cpp + ../../examples/MultiThreading/b3Win32ThreadSupport.cpp + ../../examples/MultiThreading/b3ThreadSupportInterface.cpp + +) + +IF(BUILD_CLSOCKET) + ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET) +ENDIF(BUILD_CLSOCKET) + +IF(WIN32) + LINK_LIBRARIES( + ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + IF(BUILD_ENET) + ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET) + ENDIF(BUILD_ENET) + IF(BUILD_CLSOCKET) + ADD_DEFINITIONS(-DWIN32) + ENDIF(BUILD_CLSOCKET) + +ELSE(WIN32) + IF(BUILD_ENET) + ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET) + ENDIF(BUILD_ENET) + + IF(BUILD_CLSOCKET) + ADD_DEFINITIONS(${OSDEF}) + ENDIF(BUILD_CLSOCKET) +ENDIF(WIN32) + + +IF(BUILD_ENET) + set(RobotSimulator_SRCS ${RobotSimulator_SRCS} + ../../examples/SharedMemory/PhysicsClientUDP.cpp + ../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp + ../../examples/SharedMemory/PhysicsClientUDP.h + ../../examples/SharedMemory/PhysicsClientUDP_C_API.h + ../../examples/ThirdPartyLibs/enet/win32.c + ../../examples/ThirdPartyLibs/enet/unix.c + ../../examples/ThirdPartyLibs/enet/callbacks.c + ../../examples/ThirdPartyLibs/enet/compress.c + ../../examples/ThirdPartyLibs/enet/host.c + ../../examples/ThirdPartyLibs/enet/list.c + ../../examples/ThirdPartyLibs/enet/packet.c + ../../examples/ThirdPartyLibs/enet/peer.c + ../../examples/ThirdPartyLibs/enet/protocol.c + ) +ENDIF(BUILD_ENET) + +IF(BUILD_CLSOCKET) + set(RobotSimulator_SRCS ${RobotSimulator_SRCS} + ../../examples/SharedMemory/PhysicsClientTCP.cpp + ../../examples/SharedMemory/PhysicsClientTCP.h + ../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp + ../../examples/SharedMemory/PhysicsClientTCP_C_API.h + ../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp + ../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp + ../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp + ) +ENDIF() + + +#some code to support OpenGL and Glew cross platform +IF (WIN32) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + ADD_DEFINITIONS(-DGLEW_STATIC) + LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) +ELSE(WIN32) + IF(APPLE) + find_library(COCOA NAMES Cocoa) + MESSAGE(${COCOA}) + link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) + + ELSE(APPLE) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + + LINK_LIBRARIES( pthread ${DL} ) + ENDIF(APPLE) +ENDIF(WIN32) + + +ADD_EXECUTABLE(App_TwoJoint ${RobotSimulator_SRCS}) + +SET_TARGET_PROPERTIES(App_TwoJoint PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(App_TwoJoint PROPERTIES DEBUG_POSTFIX "_d") + + +IF(WIN32) + IF(BUILD_ENET OR BUILD_CLSOCKET) + TARGET_LINK_LIBRARIES(App_TwoJoint ws2_32 ) + ENDIF(BUILD_ENET OR BUILD_CLSOCKET) +ENDIF(WIN32) + + + +TARGET_LINK_LIBRARIES(App_TwoJoint BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common) + + diff --git a/examples/TwoJoint/README.md b/examples/TwoJoint/README.md new file mode 100644 index 000000000..9b0b4bd03 --- /dev/null +++ b/examples/TwoJoint/README.md @@ -0,0 +1,13 @@ + +* Build as usual +* Run the executable and log the stdout output (for about 6 seconds, then hit escape to stop) + +``` +> App_TwoJoint > output.txt +``` + +* open `output.txt` and copy the numeric output to the clipboard +* open MATLAB (R2017A+), and type (without hitting enter) `> b3Output500 = [` +* hit ctrl+v etc. to paste long output +* type `];` and hit enter (takes a few seconds) +* run the script `compareBulletMATLAB` \ No newline at end of file diff --git a/examples/TwoJoint/TwoJointMain.cpp b/examples/TwoJoint/TwoJointMain.cpp new file mode 100644 index 000000000..d21c81d3c --- /dev/null +++ b/examples/TwoJoint/TwoJointMain.cpp @@ -0,0 +1,124 @@ +#include +#include +#include +#include + +#include "SharedMemory/PhysicsClientC_API.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Quaternion.h" + +extern const int CONTROL_RATE; +const int CONTROL_RATE = 500; + +// Bullet globals +b3PhysicsClientHandle kPhysClient = 0; +const b3Scalar FIXED_TIMESTEP = 1.0/((b3Scalar)CONTROL_RATE); +// temp vars used a lot +b3SharedMemoryCommandHandle command; +b3SharedMemoryStatusHandle statusHandle; +int statusType, ret; +b3JointInfo jointInfo; +b3JointSensorState state; +// test +int twojoint; +std::map< std::string, int > jointNameToId; + +int main(int argc, char* argv[]) { + kPhysClient = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + if (!kPhysClient) + return -1; + // visualizer + command = b3InitConfigureOpenGLVisualizer(kPhysClient); + b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0); + b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0); + b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + + b3SetTimeOut(kPhysClient, 10); + + //syncBodies is only needed when connecting to an existing physics server that has already some bodies + command = b3InitSyncBodyInfoCommand(kPhysClient); + statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + statusType = b3GetStatusType(statusHandle); + + // set fixed time step + command = b3InitPhysicsParamCommand(kPhysClient); + ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP); + statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + ret = b3PhysicsParamSetRealTimeSimulation(command, false); + statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); + + + // load test + command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf"); + int flags = URDF_USE_INERTIA_FROM_FILE; + b3LoadUrdfCommandSetFlags(command, flags); + b3LoadUrdfCommandSetUseFixedBase(command, true); + // q.setEulerZYX(0, 0, 0); + // b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]); + b3LoadUrdfCommandSetUseMultiBody(command, true); + statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); + if (statusType == CMD_URDF_LOADING_COMPLETED) { + twojoint = b3GetStatusBodyIndex(statusHandle); + } + + //disable default linear/angular damping + b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient); + double linearDamping = 0; + double angularDamping = 0; + b3ChangeDynamicsInfoSetLinearDamping(command, twojoint, linearDamping); + b3ChangeDynamicsInfoSetAngularDamping(command, twojoint, angularDamping); + statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); + + int numJoints = b3GetNumJoints(kPhysClient, twojoint); + printf("twojoint numjoints = %d\n", numJoints); + // Loop through all joints + for (int i=0; i