Merge remote-tracking branch 'bp/master'

examples/BasicDemo/CMakeLists.txt

@@ -84,7 +84,7 @@ ELSE(WIN32)
                 ADD_DEFINITIONS("-DGLEW_STATIC")
                 ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-                LINK_LIBRARIES(  pthread dl )
+                LINK_LIBRARIES(  pthread ${DL} )
         ENDIF(APPLE)
 ENDIF(WIN32)
 

examples/CMakeLists.txt

@@ -1,5 +1,16 @@
 SUBDIRS( HelloWorld BasicDemo )
 IF(BUILD_BULLET3)
+	SET(DL dl)
+	IF(CMAKE_SYSTEM_NAME MATCHES "Linux")
+		SET(OSDEF -D_LINUX)
+	ELSE(CMAKE_SYSTEM_NAME MATCHES "Linux")
+		IF(APPLE)
+			SET(OSDEF -D_DARWIN)
+		ELSE(APPLE)
+			SET(OSDEF -D_BSD)
+			SET(DL "")
+		ENDIF(APPLE)
+	ENDIF(CMAKE_SYSTEM_NAME MATCHES "Linux")
 	SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow )
 ENDIF()
 IF(BUILD_PYBULLET)

examples/ExampleBrowser/CMakeLists.txt

@@ -65,7 +65,7 @@ IF (BUILD_SHARED_LIBS)
                  TARGET_LINK_LIBRARIES(
                                 BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
                                 BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
-                                pthread dl
+                                pthread ${DL}
                         )
         ENDIF(APPLE)
 	ENDIF(WIN32)
@@ -106,7 +106,7 @@ ELSE(WIN32)
 		ADD_DEFINITIONS("-DGLEW_STATIC")
 		ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 		INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew )
-		LINK_LIBRARIES( pthread dl)
+		LINK_LIBRARIES( pthread ${DL})
 	ENDIF(APPLE)
 ENDIF(WIN32)
 

examples/OpenGLWindow/CMakeLists.txt

@@ -61,7 +61,7 @@ if (BUILD_SHARED_LIBS)
   else()
 	set (CMAKE_THREAD_PREFER_PTHREAD TRUE)
 	FIND_PACKAGE(Threads)
-   	target_link_libraries(OpenGLWindow dl ${CMAKE_THREAD_LIBS_INIT})
+	target_link_libraries(OpenGLWindow ${DL} ${CMAKE_THREAD_LIBS_INIT})
   endif()
 endif()
 

examples/RobotSimulator/CMakeLists.txt

@@ -99,11 +99,7 @@ ELSE(WIN32)
 	ENDIF(BUILD_ENET)
 
 	IF(BUILD_CLSOCKET)	
-		IF(APPLE)
+		ADD_DEFINITIONS(${OSDEF})
-			ADD_DEFINITIONS(-D_DARWIN)
-		ELSE()
-			ADD_DEFINITIONS(-D_LINUX)
-		ENDIF()
 	ENDIF(BUILD_CLSOCKET)
 ENDIF(WIN32)
 
@@ -160,7 +156,7 @@ ELSE(WIN32)
 		ADD_DEFINITIONS("-DGLEW_STATIC")
 		ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-		LINK_LIBRARIES(  pthread dl )
+		LINK_LIBRARIES(  pthread ${DL} )
 	ENDIF(APPLE)
 ENDIF(WIN32)
 

examples/SharedMemory/CMakeLists.txt

@@ -114,7 +114,7 @@ ELSE(WIN32)
 			        )
 							
         ELSE(APPLE)
-         			LINK_LIBRARIES(  pthread dl )
+                                LINK_LIBRARIES(  pthread ${DL} )
 			        ADD_EXECUTABLE(App_PhysicsServer_SharedMemory
 			                ${SharedMemory_SRCS}
 			                ../MultiThreading/b3PosixThreadSupport.cpp
@@ -181,7 +181,7 @@ ELSE(WIN32)
 			        )
 							
         ELSE(APPLE)
-         			LINK_LIBRARIES(  pthread dl )
+				LINK_LIBRARIES(  pthread ${DL} )
          			ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
               ADD_DEFINITIONS("-DGLEW_STATIC")
               ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
@@ -290,4 +290,4 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
                         SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES  RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
 ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 
-ENDIF(WIN32)
+ENDIF(WIN32)

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4639,7 +4639,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 // Set jacobian value
                                 tree->calculateJacobians(q);
                                 btInverseDynamics::mat3x jac_t(3, num_dofs);
-                                tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex, &jac_t);
+                                tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t);
                                 for (int i = 0; i < 3; ++i)
                                 {
                                     for (int j = 0; j < num_dofs; ++j)
@@ -5065,8 +5065,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                         tree->calculateJacobians(q);
                                         btInverseDynamics::mat3x jac_t(3, numDofs);
                                         btInverseDynamics::mat3x jac_r(3,numDofs);
-                                        tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
+										// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
-                                        tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
+                                        tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
+                                        tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
                                         for (int i = 0; i < 3; ++i)
                                         {
                                             for (int j = 0; j < numDofs; ++j)
@@ -6235,8 +6236,8 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 							tree->calculateJacobians(q);
 							btInverseDynamics::mat3x jac_t(3,numDofs);
 							btInverseDynamics::mat3x jac_r(3,numDofs);
-							tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
+							tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
-							tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
+							tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
 							for (int i = 0; i < 3; ++i)
 							{
 								for (int j = 0; j < numDofs; ++j)

examples/SimpleOpenGL3/CMakeLists.txt

@@ -37,7 +37,7 @@ ELSE(WIN32)
 		ADD_DEFINITIONS("-DGLEW_STATIC")
 		ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-		LINK_LIBRARIES( X11 pthread dl Xext)
+		LINK_LIBRARIES( X11 pthread ${DL} Xext)
 	ENDIF(APPLE)
 ENDIF(WIN32)
 

examples/ThirdPartyLibs/Gwen/Macros.h

@@ -4,7 +4,7 @@
 #define GWEN_MACROS_H
 #include <stdlib.h>
 #include <stdarg.h>
-#ifndef __APPLE__
+#if !defined(__APPLE__) && !defined(__OpenBSD__)
 #include <malloc.h>
 #endif //__APPLE__
 #include <memory.h>
@@ -38,7 +38,7 @@
 	#define GwenUtil_OutputDebugWideString( lpOutputString ) //wprintf( lpOutputString  )
 	#define GwenUtil_WideStringToFloat( _Str ) wcstof(_Str, NULL)
 
-#elif defined(__linux__)
+#elif defined(__linux__) || defined(__OpenBSD__)
 
 	#define GwenUtil_VSNPrintFSafe( _DstBuf, _DstSize, _MaxCount, _Format, _ArgList ) vsnprintf( _DstBuf, _DstSize, _Format, _ArgList )
 	#define GwenUtil_VSWPrintFSafe( _DstBuf, _SizeInWords, _Format, _ArgList ) vswprintf( _DstBuf, _SizeInWords, _Format, _ArgList )

examples/ThirdPartyLibs/clsocket/CMakeLists.txt

@@ -30,14 +30,7 @@ LIST(APPEND CLSOCKET_SOURCES ${CLSOCKET_HEADERS})
 # OS and compiler checks. 
 if(UNIX)
     # linux / normal unix
-    add_definitions(-D_LINUX)
+    add_definitions(${OSDEF})
-    if(CYGWIN)
-        # Special Cygwin stuff here
-    elseif(APPLE)
-        # Special Apple stuff here
-        remove_definitions(-D_LINUX)
-        add_definitions(-D_DARWIN)
-    endif()
 elseif(WIN32)
     add_definitions(-DWIN32)
     SET(PROJECT_LIBS Ws2_32.lib)

examples/ThirdPartyLibs/clsocket/src/Host.h

@@ -61,7 +61,7 @@ extern "C"
   #define __WORDSIZE 32
 #endif
 
-#if defined(_LINUX) || defined(_DARWIN)
+#if defined(_LINUX) || defined(_DARWIN) || defined(_BSD)
     typedef unsigned char  uint8;
     typedef char           int8;
     typedef unsigned short uint16;
@@ -176,7 +176,7 @@ extern "C"
 #define GETHOSTBYNAME(a)       gethostbyname(a)
 #endif
 
-#if defined(_LINUX) || defined(_DARWIN)
+#if defined(_LINUX) || defined(_DARWIN) || defined(_BSD)
 #define ACCEPT(a,b,c)          accept(a,b,c)
 #define CONNECT(a,b,c)         connect(a,b,c)
 #define CLOSE(a)               close(a)

examples/ThirdPartyLibs/clsocket/src/SimpleSocket.h

@@ -49,7 +49,7 @@
 #include <stdarg.h>
 #include <errno.h>
 
-#if defined(_LINUX) || defined (_DARWIN)
+#if defined(_LINUX) || defined (_DARWIN) || defined(_BSD)
 #include <sys/socket.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
@@ -66,7 +66,7 @@
 #ifdef _DARWIN
 #include <net/if.h>
 #endif
-#if defined(_LINUX) || defined (_DARWIN)
+#if defined(_LINUX) || defined (_DARWIN) || defined(_BSD)
 #include <sys/time.h>
 #include <sys/uio.h>
 #include <unistd.h>

examples/pybullet/CMakeLists.txt

@@ -101,11 +101,7 @@ ELSE(WIN32)
 	ENDIF(BUILD_PYBULLET_ENET)
 
 	IF(BUILD_PYBULLET_CLSOCKET)	
-		IF(APPLE)
+		ADD_DEFINITIONS(${OSDEF})
-			ADD_DEFINITIONS(-D_DARWIN)
-		ELSE()
-			ADD_DEFINITIONS(-D_LINUX)
-		ENDIF()
 	ENDIF(BUILD_PYBULLET_CLSOCKET)
 ENDIF(WIN32)
 

examples/pybullet/examples/ik_end_effector_orientation.py

@@ -0,0 +1,30 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+from time import sleep
+
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3])
+kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+
+#Joint damping coefficents. Using large values for the joints that we don't want to move.
+jd=[100.0,100.0,100.0,100.0,100.0,100.0,0.5]
+#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
+
+p.setGravity(0,0,0)
+
+while 1:
+	p.stepSimulation()
+	for i in range (1):
+		pos = [0,0,1.26]
+		orn = p.getQuaternionFromEuler([0,0,3.14])
+
+		jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
+	
+	for i in range (numJoints):
+		p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
+	sleep(0.05)