Merge pull request #1461 from avikde/TwoJoint_example

added TwoJoint example
2017-11-27 11:05:06 -08:00
parent 25628820ef fd24219f36
commit 18e7c3088f
5 changed files with 359 additions and 1 deletions
--- 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)
--- a/examples/TwoJoint/CMakeLists.txt
+++ 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)
+
+
--- a/examples/TwoJoint/README.md
+++ 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`
--- a/examples/TwoJoint/TwoJointMain.cpp
+++ b/examples/TwoJoint/TwoJointMain.cpp
@@ -0,0 +1,127 @@
+/**
+ * License: Bullet3 license
+ * Author: Avik De <avikde@gmail.com>
+ */
+#include <map>
+#include <string>
+#include <stdio.h>
+#include "../Utils/b3Clock.h"
+#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<numJoints; ++i) {
+		b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);
+		if (jointInfo.m_jointName[0]) {
+			jointNameToId[std::string(jointInfo.m_jointName)] = i;
+		} else {
+			continue;
+		}
+		// Reset before torque control - see #1459
+		command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
+		b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 0);
+		b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
+	}
+
+	// loop
+	unsigned long dtus1 = (unsigned long) 1000000.0*FIXED_TIMESTEP;
+	double simTimeS = 0;
+	double q[2], v[2];
+	while (b3CanSubmitCommand(kPhysClient)) {
+		simTimeS += 0.000001*dtus1;
+		// apply some torque
+		b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo);
+		command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
+		b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 0.5 * sin(10*simTimeS));
+		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
+
+		// get joint values
+		command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
+		b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state);
+		q[0] = state.m_jointPosition;
+		v[0] = state.m_jointVelocity;
+		b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state);
+		q[1] = state.m_jointPosition;
+		v[1] = state.m_jointVelocity;
+
+		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
+
+		// debugging output
+		printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", simTimeS, q[0], q[1], v[0], v[1]);
+		b3Clock::usleep(1000.*1000.*FIXED_TIMESTEP);
+	}
+	b3DisconnectSharedMemory(kPhysClient);
+	return 0;
+}
--- a/examples/TwoJoint/compareBulletMATLAB.m
+++ b/examples/TwoJoint/compareBulletMATLAB.m
@@ -0,0 +1,39 @@
+% License: Bullet3 license
+% Author: Avik De <avikde@gmail.com>
+
+robot = importrobot('../../data/TwoJointRobot_wo_fixedJoints.urdf');
+show(robot)
+
+robot.DataFormat = 'column';
+
+X0 = zeros(4,1);
+
+options = odeset('MaxStep',5e-3);
+[t,X] = ode45(@(t, X) myDyn(t, X, robot), [0,5], X0, options);
+
+
+subplot(211)
+hold all
+plot(t, X(:,1))
+plot(b3output500(:,1), b3output500(:,2), '--')
+plot(t, X(:,2))
+plot(b3output500(:,1), b3output500(:,3), '--')
+legend('m1','b1','m2','b2')
+ylabel('pos')
+subplot(212)
+hold all
+plot(t, X(:,3))
+plot(b3output500(:,1), b3output500(:,4), '--')
+plot(t, X(:,4))
+plot(b3output500(:,1), b3output500(:,5), '--')
+legend('m1','b1','m2','b2')
+ylabel('vel')
+
+hold off
+
+function Xd = myDyn(t, X, robot)
+	q = X(1:2);
+	qd = X(3:4);
+	qdd = forwardDynamics(robot, q, qd, [0;0.5*sin(10*t)]);
+	Xd = [qd;qdd];
+end