2015-11-23 04:50:32 +00:00
# include "PhysicsServerCommandProcessor.h"
# include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
# include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
# include "../Importers/ImportURDFDemo/URDF2Bullet.h"
2016-08-10 01:40:12 +00:00
# include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
2016-06-01 05:55:13 +00:00
# include "TinyRendererVisualShapeConverter.h"
2015-11-23 04:50:32 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
# include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
# include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
# include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
2016-08-23 01:14:29 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
2016-08-26 17:35:10 +00:00
# include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
2016-09-29 19:07:54 +00:00
# include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
2016-08-10 01:40:12 +00:00
# include "LinearMath/btHashMap.h"
# include "BulletInverseDynamics/MultiBodyTree.hpp"
2016-09-08 22:15:58 +00:00
# include "IKTrajectoryHelper.h"
2015-11-23 04:50:32 +00:00
# include "btBulletDynamicsCommon.h"
# include "LinearMath/btTransform.h"
# include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
# include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
# include "LinearMath/btSerializer.h"
# include "Bullet3Common/b3Logging.h"
# include "../CommonInterfaces/CommonGUIHelperInterface.h"
# include "SharedMemoryCommands.h"
2016-10-19 00:38:43 +00:00
# ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
# include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
# include "BulletSoftBody/btSoftBodySolvers.h"
# include "BulletSoftBody/btSoftBodyHelpers.h"
2016-10-18 06:40:38 +00:00
# include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
2016-10-12 18:51:04 +00:00
# include "../SoftDemo/BunnyMesh.h"
2016-10-19 00:38:43 +00:00
# else
# include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
# endif
2015-11-23 04:50:32 +00:00
2016-10-14 22:06:09 +00:00
2016-10-04 15:53:59 +00:00
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
2016-09-15 23:57:00 +00:00
btVector3 gLastPickPos ( 0 , 0 , 0 ) ;
2016-10-04 15:53:59 +00:00
bool gCloseToKuka = false ;
2016-09-19 14:02:43 +00:00
bool gEnableRealTimeSimVR = false ;
2016-10-23 14:14:50 +00:00
bool gCreateDefaultRobotAssets = false ;
int gInternalSimFlags = 0 ;
2016-10-09 01:40:09 +00:00
2016-09-22 15:50:28 +00:00
int gCreateObjectSimVR = - 1 ;
btScalar simTimeScalingFactor = 1 ;
2016-09-28 18:17:11 +00:00
btScalar gRhsClamp = 1.f ;
2016-09-15 23:57:00 +00:00
2015-11-23 04:50:32 +00:00
struct UrdfLinkNameMapUtil
{
btMultiBody * m_mb ;
btDefaultSerializer * m_memSerializer ;
UrdfLinkNameMapUtil ( ) : m_mb ( 0 ) , m_memSerializer ( 0 )
{
}
2016-07-17 04:29:31 +00:00
virtual ~ UrdfLinkNameMapUtil ( )
{
delete m_memSerializer ;
}
2015-11-23 04:50:32 +00:00
} ;
struct SharedMemoryDebugDrawer : public btIDebugDraw
{
int m_debugMode ;
btAlignedObjectArray < SharedMemLines > m_lines2 ;
SharedMemoryDebugDrawer ( )
: m_debugMode ( 0 )
{
}
virtual void drawContactPoint ( const btVector3 & PointOnB , const btVector3 & normalOnB , btScalar distance , int lifeTime , const btVector3 & color )
{
}
virtual void reportErrorWarning ( const char * warningString )
{
}
virtual void draw3dText ( const btVector3 & location , const char * textString )
{
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
virtual void setDebugMode ( int debugMode )
{
m_debugMode = debugMode ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
virtual int getDebugMode ( ) const
{
return m_debugMode ;
}
virtual void drawLine ( const btVector3 & from , const btVector3 & to , const btVector3 & color )
{
SharedMemLines line ;
line . m_from = from ;
line . m_to = to ;
line . m_color = color ;
m_lines2 . push_back ( line ) ;
}
} ;
struct InteralBodyData
{
btMultiBody * m_multiBody ;
btRigidBody * m_rigidBody ;
int m_testData ;
btTransform m_rootLocalInertialFrame ;
2016-04-29 21:45:15 +00:00
btAlignedObjectArray < btTransform > m_linkLocalInertialFrames ;
2015-11-23 04:50:32 +00:00
InteralBodyData ( )
: m_multiBody ( 0 ) ,
m_rigidBody ( 0 ) ,
m_testData ( 0 )
{
m_rootLocalInertialFrame . setIdentity ( ) ;
}
} ;
///todo: templatize this
struct InternalBodyHandle : public InteralBodyData
{
BT_DECLARE_ALIGNED_ALLOCATOR ( ) ;
int m_nextFreeHandle ;
2016-06-24 14:31:17 +00:00
void SetNextFree ( int next )
2015-11-23 04:50:32 +00:00
{
m_nextFreeHandle = next ;
}
2016-06-24 14:31:17 +00:00
int GetNextFree ( ) const
2015-11-23 04:50:32 +00:00
{
return m_nextFreeHandle ;
}
} ;
class btCommandChunk
{
public :
int m_chunkCode ;
int m_length ;
void * m_oldPtr ;
int m_dna_nr ;
int m_number ;
} ;
class bCommandChunkPtr4
{
public :
bCommandChunkPtr4 ( ) { }
int code ;
int len ;
union
{
int m_uniqueInt ;
} ;
int dna_nr ;
int nr ;
} ;
// ----------------------------------------------------- //
class bCommandChunkPtr8
{
public :
bCommandChunkPtr8 ( ) { }
int code , len ;
union
{
int m_uniqueInts [ 2 ] ;
} ;
int dna_nr , nr ;
} ;
struct CommandLogger
{
FILE * m_file ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
void writeHeader ( unsigned char * buffer ) const
{
# ifdef BT_USE_DOUBLE_PRECISION
memcpy ( buffer , " BT3CMDd " , 7 ) ;
# else
memcpy ( buffer , " BT3CMDf " , 7 ) ;
# endif //BT_USE_DOUBLE_PRECISION
int littleEndian = 1 ;
littleEndian = ( ( char * ) & littleEndian ) [ 0 ] ;
if ( sizeof ( void * ) = = 8 )
{
buffer [ 7 ] = ' - ' ;
} else
{
buffer [ 7 ] = ' _ ' ;
}
if ( littleEndian )
{
buffer [ 8 ] = ' v ' ;
} else
{
buffer [ 8 ] = ' V ' ;
}
buffer [ 9 ] = 0 ;
buffer [ 10 ] = 0 ;
buffer [ 11 ] = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int ver = btGetVersion ( ) ;
if ( ver > = 0 & & ver < 999 )
{
sprintf ( ( char * ) & buffer [ 9 ] , " %d " , ver ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
void logCommand ( const SharedMemoryCommand & command )
{
btCommandChunk chunk ;
chunk . m_chunkCode = command . m_type ;
chunk . m_oldPtr = 0 ;
chunk . m_dna_nr = 0 ;
chunk . m_length = sizeof ( SharedMemoryCommand ) ;
chunk . m_number = 1 ;
fwrite ( ( const char * ) & chunk , sizeof ( btCommandChunk ) , 1 , m_file ) ;
fwrite ( ( const char * ) & command , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
}
CommandLogger ( const char * fileName )
{
m_file = fopen ( fileName , " wb " ) ;
unsigned char buf [ 15 ] ;
buf [ 12 ] = 12 ;
buf [ 13 ] = 13 ;
buf [ 14 ] = 14 ;
writeHeader ( buf ) ;
fwrite ( buf , 12 , 1 , m_file ) ;
}
virtual ~ CommandLogger ( )
{
fclose ( m_file ) ;
}
} ;
struct CommandLogPlayback
{
unsigned char m_header [ 12 ] ;
FILE * m_file ;
bool m_bitsVary ;
bool m_fileIs64bit ;
CommandLogPlayback ( const char * fileName )
{
m_file = fopen ( fileName , " rb " ) ;
if ( m_file )
{
fread ( m_header , 12 , 1 , m_file ) ;
}
unsigned char c = m_header [ 7 ] ;
m_fileIs64bit = ( c = = ' - ' ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
const bool VOID_IS_8 = ( ( sizeof ( void * ) = = 8 ) ) ;
m_bitsVary = ( VOID_IS_8 ! = m_fileIs64bit ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
virtual ~ CommandLogPlayback ( )
{
if ( m_file )
{
fclose ( m_file ) ;
m_file = 0 ;
}
}
bool processNextCommand ( SharedMemoryCommand * cmd )
{
if ( m_file )
{
size_t s = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_fileIs64bit )
{
bCommandChunkPtr8 chunk8 ;
s = fread ( ( void * ) & chunk8 , sizeof ( bCommandChunkPtr8 ) , 1 , m_file ) ;
} else
{
bCommandChunkPtr4 chunk4 ;
s = fread ( ( void * ) & chunk4 , sizeof ( bCommandChunkPtr4 ) , 1 , m_file ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( s = = 1 )
{
s = fread ( cmd , sizeof ( SharedMemoryCommand ) , 1 , m_file ) ;
return ( s = = 1 ) ;
}
}
return false ;
}
} ;
2016-10-13 06:03:36 +00:00
struct SaveWorldObjectData
{
b3AlignedObjectArray < int > m_bodyUniqueIds ;
std : : string m_fileName ;
} ;
2015-11-23 04:50:32 +00:00
struct PhysicsServerCommandProcessorInternalData
{
///handle management
btAlignedObjectArray < InternalBodyHandle > m_bodyHandles ;
int m_numUsedHandles ; // number of active handles
int m_firstFreeHandle ; // free handles list
InternalBodyHandle * getHandle ( int handle )
{
btAssert ( handle > = 0 ) ;
btAssert ( handle < m_bodyHandles . size ( ) ) ;
if ( ( handle < 0 ) | | ( handle > = m_bodyHandles . size ( ) ) )
{
return 0 ;
}
return & m_bodyHandles [ handle ] ;
}
const InternalBodyHandle * getHandle ( int handle ) const
{
return & m_bodyHandles [ handle ] ;
}
void increaseHandleCapacity ( int extraCapacity )
{
int curCapacity = m_bodyHandles . size ( ) ;
btAssert ( curCapacity = = m_numUsedHandles ) ;
int newCapacity = curCapacity + extraCapacity ;
m_bodyHandles . resize ( newCapacity ) ;
{
for ( int i = curCapacity ; i < newCapacity ; i + + )
m_bodyHandles [ i ] . SetNextFree ( i + 1 ) ;
m_bodyHandles [ newCapacity - 1 ] . SetNextFree ( - 1 ) ;
}
m_firstFreeHandle = curCapacity ;
}
void initHandles ( )
{
m_numUsedHandles = 0 ;
m_firstFreeHandle = - 1 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
increaseHandleCapacity ( 1 ) ;
}
void exitHandles ( )
{
m_bodyHandles . resize ( 0 ) ;
m_firstFreeHandle = - 1 ;
m_numUsedHandles = 0 ;
}
int allocHandle ( )
{
btAssert ( m_firstFreeHandle > = 0 ) ;
int handle = m_firstFreeHandle ;
m_firstFreeHandle = getHandle ( handle ) - > GetNextFree ( ) ;
m_numUsedHandles + + ;
if ( m_firstFreeHandle < 0 )
{
int curCapacity = m_bodyHandles . size ( ) ;
int additionalCapacity = m_bodyHandles . size ( ) ;
increaseHandleCapacity ( additionalCapacity ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
getHandle ( handle ) - > SetNextFree ( m_firstFreeHandle ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return handle ;
}
void freeHandle ( int handle )
{
btAssert ( handle > = 0 ) ;
getHandle ( handle ) - > SetNextFree ( m_firstFreeHandle ) ;
m_firstFreeHandle = handle ;
m_numUsedHandles - - ;
}
///end handle management
2016-06-24 14:31:17 +00:00
2016-07-18 06:50:11 +00:00
bool m_allowRealTimeSimulation ;
bool m_hasGround ;
2016-06-24 14:31:17 +00:00
2016-09-08 22:15:58 +00:00
btMultiBodyFixedConstraint * m_gripperRigidbodyFixed ;
2016-09-09 21:30:37 +00:00
btMultiBody * m_gripperMultiBody ;
2016-09-25 23:05:53 +00:00
btMultiBodyFixedConstraint * m_kukaGripperFixed ;
btMultiBody * m_kukaGripperMultiBody ;
2016-09-29 19:07:54 +00:00
btMultiBodyPoint2Point * m_kukaGripperRevolute1 ;
btMultiBodyPoint2Point * m_kukaGripperRevolute2 ;
2016-10-14 22:06:09 +00:00
2016-09-15 23:57:00 +00:00
int m_huskyId ;
2016-09-23 02:48:26 +00:00
int m_KukaId ;
int m_sphereId ;
2016-09-25 23:05:53 +00:00
int m_gripperId ;
2015-11-23 04:50:32 +00:00
CommandLogger * m_commandLogger ;
CommandLogPlayback * m_logPlayback ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btScalar m_physicsDeltaTime ;
2016-08-24 21:25:06 +00:00
btScalar m_numSimulationSubSteps ;
2015-11-23 04:50:32 +00:00
btAlignedObjectArray < btMultiBodyJointFeedback * > m_multiBodyJointFeedbacks ;
2016-08-10 01:40:12 +00:00
btHashMap < btHashPtr , btInverseDynamics : : MultiBodyTree * > m_inverseDynamicsBodies ;
2016-09-08 22:15:58 +00:00
btHashMap < btHashPtr , IKTrajectoryHelper * > m_inverseKinematicsHelpers ;
2016-10-13 06:03:36 +00:00
b3AlignedObjectArray < SaveWorldObjectData > m_saveWorldBodyData ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btAlignedObjectArray < btBulletWorldImporter * > m_worldImporters ;
btAlignedObjectArray < UrdfLinkNameMapUtil * > m_urdfLinkNameMapper ;
btAlignedObjectArray < std : : string * > m_strings ;
btAlignedObjectArray < btCollisionShape * > m_collisionShapes ;
btBroadphaseInterface * m_broadphase ;
btCollisionDispatcher * m_dispatcher ;
btMultiBodyConstraintSolver * m_solver ;
btDefaultCollisionConfiguration * m_collisionConfiguration ;
2016-10-19 00:38:43 +00:00
# ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
btSoftMultiBodyDynamicsWorld * m_dynamicsWorld ;
2016-10-19 00:38:43 +00:00
btSoftBodySolver * m_softbodySolver ;
2016-10-12 22:34:45 +00:00
btSoftBodyWorldInfo m_softBodyWorldInfo ;
2016-10-19 00:38:43 +00:00
# else
btMultiBodyDynamicsWorld * m_dynamicsWorld ;
# endif
2015-11-23 04:50:32 +00:00
SharedMemoryDebugDrawer * m_remoteDebugDrawer ;
2016-10-19 00:38:43 +00:00
2016-09-01 20:30:07 +00:00
btAlignedObjectArray < b3ContactPointData > m_cachedContactPoints ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
btAlignedObjectArray < int > m_sdfRecentLoadedBodies ;
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
struct GUIHelperInterface * m_guiHelper ;
int m_sharedMemoryKey ;
bool m_verboseOutput ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//data for picking objects
class btRigidBody * m_pickedBody ;
class btTypedConstraint * m_pickedConstraint ;
class btMultiBodyPoint2Point * m_pickingMultiBodyPoint2Point ;
btVector3 m_oldPickingPos ;
btVector3 m_hitPos ;
btScalar m_oldPickingDist ;
bool m_prevCanSleep ;
2016-06-01 05:55:13 +00:00
TinyRendererVisualShapeConverter m_visualConverter ;
2015-11-23 04:50:32 +00:00
PhysicsServerCommandProcessorInternalData ( )
2016-07-18 06:50:11 +00:00
: m_hasGround ( false ) ,
2016-09-08 22:15:58 +00:00
m_gripperRigidbodyFixed ( 0 ) ,
2016-09-09 21:30:37 +00:00
m_gripperMultiBody ( 0 ) ,
2016-10-14 22:06:09 +00:00
m_kukaGripperFixed ( 0 ) ,
m_kukaGripperMultiBody ( 0 ) ,
m_kukaGripperRevolute1 ( 0 ) ,
m_kukaGripperRevolute2 ( 0 ) ,
2016-07-18 06:50:11 +00:00
m_allowRealTimeSimulation ( false ) ,
2016-09-23 02:48:26 +00:00
m_huskyId ( - 1 ) ,
m_KukaId ( - 1 ) ,
m_sphereId ( - 1 ) ,
2016-09-25 23:05:53 +00:00
m_gripperId ( - 1 ) ,
2015-11-23 04:50:32 +00:00
m_commandLogger ( 0 ) ,
m_logPlayback ( 0 ) ,
m_physicsDeltaTime ( 1. / 240. ) ,
2016-08-24 21:25:06 +00:00
m_numSimulationSubSteps ( 0 ) ,
2015-11-23 04:50:32 +00:00
m_dynamicsWorld ( 0 ) ,
m_remoteDebugDrawer ( 0 ) ,
m_guiHelper ( 0 ) ,
m_sharedMemoryKey ( SHARED_MEMORY_KEY ) ,
m_verboseOutput ( false ) ,
m_pickedBody ( 0 ) ,
m_pickedConstraint ( 0 ) ,
m_pickingMultiBodyPoint2Point ( 0 )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
initHandles ( ) ;
#if 0
btAlignedObjectArray < int > bla ;
for ( int i = 0 ; i < 1024 ; i + + )
{
int handle = allocHandle ( ) ;
bla . push_back ( handle ) ;
InternalBodyHandle * body = getHandle ( handle ) ;
InteralBodyData * body2 = body ;
}
for ( int i = 0 ; i < bla . size ( ) ; i + + )
{
freeHandle ( bla [ i ] ) ;
}
bla . resize ( 0 ) ;
for ( int i = 0 ; i < 1024 ; i + + )
{
int handle = allocHandle ( ) ;
bla . push_back ( handle ) ;
InternalBodyHandle * body = getHandle ( handle ) ;
InteralBodyData * body2 = body ;
}
for ( int i = 0 ; i < bla . size ( ) ; i + + )
{
freeHandle ( bla [ i ] ) ;
}
bla . resize ( 0 ) ;
for ( int i = 0 ; i < 1024 ; i + + )
{
int handle = allocHandle ( ) ;
bla . push_back ( handle ) ;
InternalBodyHandle * body = getHandle ( handle ) ;
InteralBodyData * body2 = body ;
}
for ( int i = 0 ; i < bla . size ( ) ; i + + )
{
freeHandle ( bla [ i ] ) ;
}
# endif
}
2016-09-13 22:37:46 +00:00
btInverseDynamics : : MultiBodyTree * findOrCreateTree ( btMultiBody * multiBody )
{
btInverseDynamics : : MultiBodyTree * tree = 0 ;
btInverseDynamics : : MultiBodyTree * * treePtrPtr =
m_inverseDynamicsBodies . find ( multiBody ) ;
if ( treePtrPtr )
{
tree = * treePtrPtr ;
}
else
{
btInverseDynamics : : btMultiBodyTreeCreator id_creator ;
if ( - 1 = = id_creator . createFromBtMultiBody ( multiBody , false ) )
{
}
else
{
tree = btInverseDynamics : : CreateMultiBodyTree ( id_creator ) ;
m_inverseDynamicsBodies . insert ( multiBody , tree ) ;
}
}
return tree ;
}
2015-11-23 04:50:32 +00:00
2016-09-13 22:37:46 +00:00
2015-11-23 04:50:32 +00:00
} ;
void PhysicsServerCommandProcessor : : setGuiHelper ( struct GUIHelperInterface * guiHelper )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( guiHelper )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
guiHelper - > createPhysicsDebugDrawer ( m_data - > m_dynamicsWorld ) ;
} else
{
if ( m_data - > m_guiHelper & & m_data - > m_dynamicsWorld & & m_data - > m_dynamicsWorld - > getDebugDrawer ( ) )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > setDebugDrawer ( 0 ) ;
}
}
m_data - > m_guiHelper = guiHelper ;
2016-07-18 06:50:11 +00:00
2016-09-01 17:45:14 +00:00
2016-07-18 06:50:11 +00:00
2015-11-23 04:50:32 +00:00
}
PhysicsServerCommandProcessor : : PhysicsServerCommandProcessor ( )
{
m_data = new PhysicsServerCommandProcessorInternalData ( ) ;
createEmptyDynamicsWorld ( ) ;
2016-09-24 18:25:05 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_linearSlop = 0.00001 ;
2016-09-22 15:50:28 +00:00
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_numIterations = 100 ;
2016-09-01 17:45:14 +00:00
2015-11-23 04:50:32 +00:00
}
PhysicsServerCommandProcessor : : ~ PhysicsServerCommandProcessor ( )
{
deleteDynamicsWorld ( ) ;
if ( m_data - > m_commandLogger )
{
delete m_data - > m_commandLogger ;
m_data - > m_commandLogger = 0 ;
}
delete m_data ;
}
void PhysicsServerCommandProcessor : : createEmptyDynamicsWorld ( )
{
2016-10-17 20:01:04 +00:00
///collision configuration contains default setup for memory, collision setup
//m_collisionConfiguration->setConvexConvexMultipointIterations();
2016-10-19 00:38:43 +00:00
# ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
m_data - > m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration ( ) ;
2016-10-19 00:38:43 +00:00
# else
m_data - > m_collisionConfiguration = new btDefaultCollisionConfiguration ( ) ;
# endif
2016-10-17 20:01:04 +00:00
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_data - > m_dispatcher = new btCollisionDispatcher ( m_data - > m_collisionConfiguration ) ;
2016-10-12 22:34:45 +00:00
2016-10-17 20:01:04 +00:00
m_data - > m_broadphase = new btDbvtBroadphase ( ) ;
2016-10-12 18:51:04 +00:00
2016-10-17 20:01:04 +00:00
m_data - > m_solver = new btMultiBodyConstraintSolver ;
2016-10-12 18:51:04 +00:00
2016-10-19 00:38:43 +00:00
# ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-17 20:01:04 +00:00
m_data - > m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld ( m_data - > m_dispatcher , m_data - > m_broadphase , m_data - > m_solver , m_data - > m_collisionConfiguration ) ;
2016-10-19 00:38:43 +00:00
# else
m_data - > m_dynamicsWorld = new btMultiBodyDynamicsWorld ( m_data - > m_dispatcher , m_data - > m_broadphase , m_data - > m_solver , m_data - > m_collisionConfiguration ) ;
# endif
2016-10-17 20:01:04 +00:00
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data - > m_dynamicsWorld - > getCollisionObjectArray ( ) . reserve ( 8192 ) ;
m_data - > m_remoteDebugDrawer = new SharedMemoryDebugDrawer ( ) ;
m_data - > m_dynamicsWorld - > setGravity ( btVector3 ( 0 , 0 , 0 ) ) ;
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp2 = 0.08 ;
2016-10-12 18:51:04 +00:00
2015-11-23 04:50:32 +00:00
}
2016-08-10 01:40:12 +00:00
void PhysicsServerCommandProcessor : : deleteCachedInverseDynamicsBodies ( )
{
for ( int i = 0 ; i < m_data - > m_inverseDynamicsBodies . size ( ) ; i + + )
{
btInverseDynamics : : MultiBodyTree * * treePtrPtr = m_data - > m_inverseDynamicsBodies . getAtIndex ( i ) ;
if ( treePtrPtr )
{
btInverseDynamics : : MultiBodyTree * tree = * treePtrPtr ;
delete tree ;
}
}
m_data - > m_inverseDynamicsBodies . clear ( ) ;
}
2015-11-23 04:50:32 +00:00
void PhysicsServerCommandProcessor : : deleteDynamicsWorld ( )
{
2016-08-10 01:40:12 +00:00
deleteCachedInverseDynamicsBodies ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_multiBodyJointFeedbacks . size ( ) ; i + + )
{
delete m_data - > m_multiBodyJointFeedbacks [ i ] ;
}
m_data - > m_multiBodyJointFeedbacks . clear ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_worldImporters . size ( ) ; i + + )
{
delete m_data - > m_worldImporters [ i ] ;
}
m_data - > m_worldImporters . clear ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_urdfLinkNameMapper . size ( ) ; i + + )
{
delete m_data - > m_urdfLinkNameMapper [ i ] ;
}
m_data - > m_urdfLinkNameMapper . clear ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < m_data - > m_strings . size ( ) ; i + + )
{
delete m_data - > m_strings [ i ] ;
}
m_data - > m_strings . clear ( ) ;
2016-04-11 23:42:02 +00:00
btAlignedObjectArray < btTypedConstraint * > constraints ;
btAlignedObjectArray < btMultiBodyConstraint * > mbconstraints ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_dynamicsWorld )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int i ;
for ( i = m_data - > m_dynamicsWorld - > getNumConstraints ( ) - 1 ; i > = 0 ; i - - )
{
2016-04-11 23:42:02 +00:00
btTypedConstraint * constraint = m_data - > m_dynamicsWorld - > getConstraint ( i ) ;
constraints . push_back ( constraint ) ;
m_data - > m_dynamicsWorld - > removeConstraint ( constraint ) ;
2015-11-23 04:50:32 +00:00
}
2016-04-11 23:42:02 +00:00
for ( i = m_data - > m_dynamicsWorld - > getNumMultiBodyConstraints ( ) - 1 ; i > = 0 ; i - - )
{
btMultiBodyConstraint * mbconstraint = m_data - > m_dynamicsWorld - > getMultiBodyConstraint ( i ) ;
mbconstraints . push_back ( mbconstraint ) ;
m_data - > m_dynamicsWorld - > removeMultiBodyConstraint ( mbconstraint ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
for ( i = m_data - > m_dynamicsWorld - > getNumCollisionObjects ( ) - 1 ; i > = 0 ; i - - )
{
btCollisionObject * obj = m_data - > m_dynamicsWorld - > getCollisionObjectArray ( ) [ i ] ;
btRigidBody * body = btRigidBody : : upcast ( obj ) ;
if ( body & & body - > getMotionState ( ) )
{
delete body - > getMotionState ( ) ;
}
m_data - > m_dynamicsWorld - > removeCollisionObject ( obj ) ;
delete obj ;
}
2016-04-11 23:42:02 +00:00
for ( i = m_data - > m_dynamicsWorld - > getNumMultibodies ( ) - 1 ; i > = 0 ; i - - )
{
btMultiBody * mb = m_data - > m_dynamicsWorld - > getMultiBody ( i ) ;
m_data - > m_dynamicsWorld - > removeMultiBody ( mb ) ;
delete mb ;
}
2015-11-23 04:50:32 +00:00
}
2016-04-11 23:42:02 +00:00
for ( int i = 0 ; i < constraints . size ( ) ; i + + )
{
delete constraints [ i ] ;
}
constraints . clear ( ) ;
for ( int i = 0 ; i < mbconstraints . size ( ) ; i + + )
{
delete mbconstraints [ i ] ;
}
mbconstraints . clear ( ) ;
//delete collision shapes
2015-11-23 04:50:32 +00:00
for ( int j = 0 ; j < m_data - > m_collisionShapes . size ( ) ; j + + )
{
btCollisionShape * shape = m_data - > m_collisionShapes [ j ] ;
delete shape ;
}
m_data - > m_collisionShapes . clear ( ) ;
delete m_data - > m_dynamicsWorld ;
m_data - > m_dynamicsWorld = 0 ;
delete m_data - > m_remoteDebugDrawer ;
m_data - > m_remoteDebugDrawer = 0 ;
delete m_data - > m_solver ;
m_data - > m_solver = 0 ;
delete m_data - > m_broadphase ;
m_data - > m_broadphase = 0 ;
delete m_data - > m_dispatcher ;
m_data - > m_dispatcher = 0 ;
delete m_data - > m_collisionConfiguration ;
m_data - > m_collisionConfiguration = 0 ;
}
bool PhysicsServerCommandProcessor : : supportsJointMotor ( btMultiBody * mb , int mbLinkIndex )
{
bool canHaveMotor = ( mb - > getLink ( mbLinkIndex ) . m_jointType = = btMultibodyLink : : eRevolute
| | mb - > getLink ( mbLinkIndex ) . m_jointType = = btMultibodyLink : : ePrismatic ) ;
return canHaveMotor ;
}
//for testing, create joint motors for revolute and prismatic joints
void PhysicsServerCommandProcessor : : createJointMotors ( btMultiBody * mb )
{
int numLinks = mb - > getNumLinks ( ) ;
for ( int i = 0 ; i < numLinks ; i + + )
{
int mbLinkIndex = i ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( supportsJointMotor ( mb , mbLinkIndex ) )
{
2016-09-23 06:21:24 +00:00
float maxMotorImpulse = 1.f ;
2015-11-23 04:50:32 +00:00
int dof = 0 ;
btScalar desiredVelocity = 0.f ;
btMultiBodyJointMotor * motor = new btMultiBodyJointMotor ( mb , mbLinkIndex , dof , desiredVelocity , maxMotorImpulse ) ;
2016-08-18 23:48:14 +00:00
motor - > setPositionTarget ( 0 , 0 ) ;
2016-08-18 20:10:28 +00:00
motor - > setVelocityTarget ( 0 , 1 ) ;
2016-09-28 23:07:55 +00:00
//motor->setRhsClamp(gRhsClamp);
2015-11-23 04:50:32 +00:00
//motor->setMaxAppliedImpulse(0);
2016-06-17 01:46:34 +00:00
mb - > getLink ( mbLinkIndex ) . m_userPtr = motor ;
2015-11-23 04:50:32 +00:00
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( motor ) ;
2016-04-09 01:17:17 +00:00
motor - > finalizeMultiDof ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
}
}
2016-08-17 00:56:30 +00:00
bool PhysicsServerCommandProcessor : : loadSdf ( const char * fileName , char * bufferServerToClient , int bufferSizeInBytes , bool useMultiBody )
2016-06-13 17:11:28 +00:00
{
btAssert ( m_data - > m_dynamicsWorld ) ;
if ( ! m_data - > m_dynamicsWorld )
{
b3Error ( " loadSdf: No valid m_dynamicsWorld " ) ;
return false ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
m_data - > m_sdfRecentLoadedBodies . clear ( ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , & m_data - > m_visualConverter ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
bool useFixedBase = false ;
bool loadOk = u2b . loadSDF ( fileName , useFixedBase ) ;
if ( loadOk )
{
for ( int i = 0 ; i < u2b . getNumAllocatedCollisionShapes ( ) ; i + + )
{
btCollisionShape * shape = u2b . getAllocatedCollisionShape ( i ) ;
m_data - > m_collisionShapes . push_back ( shape ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
btTransform rootTrans ;
rootTrans . setIdentity ( ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " loaded %s OK! " , fileName ) ;
}
2016-10-13 06:03:36 +00:00
SaveWorldObjectData sd ;
sd . m_fileName = fileName ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
for ( int m = 0 ; m < u2b . getNumModels ( ) ; m + + )
{
u2b . activateModel ( m ) ;
btMultiBody * mb = 0 ;
2016-08-23 01:14:29 +00:00
btRigidBody * rb = 0 ;
2016-06-13 17:11:28 +00:00
//get a body index
int bodyUniqueId = m_data - > allocHandle ( ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
2016-09-01 20:30:07 +00:00
2016-10-13 06:03:36 +00:00
sd . m_bodyUniqueIds . push_back ( bodyUniqueId ) ;
2016-09-01 20:30:07 +00:00
2016-08-11 21:55:30 +00:00
u2b . setBodyUniqueId ( bodyUniqueId ) ;
2016-06-13 17:11:28 +00:00
{
btScalar mass = 0 ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
int urdfLinkIndex = u2b . getRootLinkIndex ( ) ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , bodyHandle - > m_rootLocalInertialFrame ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b . getRootLinkIndex ( ) ;
b3Printf ( " urdf root link index = %d \n " , rootLinkIndex ) ;
MyMultiBodyCreator creation ( m_data - > m_guiHelper ) ;
u2b . getRootTransformInWorld ( rootTrans ) ;
2016-08-29 22:09:18 +00:00
ConvertURDF2Bullet ( u2b , creation , rootTrans , m_data - > m_dynamicsWorld , useMultiBody , u2b . getPathPrefix ( ) , CUF_USE_SDF ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
mb = creation . getBulletMultiBody ( ) ;
2016-08-23 01:14:29 +00:00
rb = creation . getRigidBody ( ) ;
2016-09-01 20:30:07 +00:00
if ( rb )
rb - > setUserIndex2 ( bodyUniqueId ) ;
if ( mb )
mb - > setUserIndex2 ( bodyUniqueId ) ;
2016-10-13 06:03:36 +00:00
2016-06-13 17:11:28 +00:00
if ( mb )
{
bodyHandle - > m_multiBody = mb ;
2016-09-01 20:30:07 +00:00
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
m_data - > m_sdfRecentLoadedBodies . push_back ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
createJointMotors ( mb ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
bodyHandle - > m_linkLocalInertialFrames . reserve ( mb - > getNumLinks ( ) ) ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
//disable serialization of the collision objects
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
int urdfLinkIndex = creation . m_mb2urdfLink [ i ] ;
btScalar mass ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
btTransform localInertialFrame ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , localInertialFrame ) ;
bodyHandle - > m_linkLocalInertialFrames . push_back ( localInertialFrame ) ;
std : : string * linkName = new std : : string ( u2b . getLinkName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( linkName ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
mb - > getLink ( i ) . m_linkName = linkName - > c_str ( ) ;
std : : string * jointName = new std : : string ( u2b . getJointName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( jointName ) ;
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
mb - > getLink ( i ) . m_jointName = jointName - > c_str ( ) ;
}
std : : string * baseName = new std : : string ( u2b . getLinkName ( u2b . getRootLinkIndex ( ) ) ) ;
m_data - > m_strings . push_back ( baseName ) ;
mb - > setBaseName ( baseName - > c_str ( ) ) ;
} else
{
b3Warning ( " No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later. " ) ;
2016-08-23 01:14:29 +00:00
bodyHandle - > m_rigidBody = rb ;
2016-06-13 17:11:28 +00:00
}
2016-06-24 14:31:17 +00:00
2016-06-13 17:11:28 +00:00
}
2016-10-13 06:03:36 +00:00
m_data - > m_saveWorldBodyData . push_back ( sd ) ;
2016-06-13 17:11:28 +00:00
}
2016-06-24 14:31:17 +00:00
return loadOk ;
2016-06-13 17:11:28 +00:00
}
2015-11-23 04:50:32 +00:00
2016-09-13 22:37:46 +00:00
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : loadUrdf ( const char * fileName , const btVector3 & pos , const btQuaternion & orn ,
bool useMultiBody , bool useFixedBase , int * bodyUniqueIdPtr , char * bufferServerToClient , int bufferSizeInBytes )
{
btAssert ( m_data - > m_dynamicsWorld ) ;
if ( ! m_data - > m_dynamicsWorld )
{
b3Error ( " loadUrdf: No valid m_dynamicsWorld " ) ;
return false ;
}
2016-05-20 01:37:15 +00:00
2016-06-24 14:31:17 +00:00
2016-06-01 05:55:13 +00:00
BulletURDFImporter u2b ( m_data - > m_guiHelper , & m_data - > m_visualConverter ) ;
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
bool loadOk = u2b . loadURDF ( fileName , useFixedBase ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( loadOk )
{
//get a body index
int bodyUniqueId = m_data - > allocHandle ( ) ;
if ( bodyUniqueIdPtr )
* bodyUniqueIdPtr = bodyUniqueId ;
2016-10-13 06:03:36 +00:00
//quick prototype of 'save world' for crude world editing
{
SaveWorldObjectData sd ;
sd . m_fileName = fileName ;
sd . m_bodyUniqueIds . push_back ( bodyUniqueId ) ;
m_data - > m_saveWorldBodyData . push_back ( sd ) ;
}
2016-08-11 21:55:30 +00:00
u2b . setBodyUniqueId ( bodyUniqueId ) ;
2015-11-23 04:50:32 +00:00
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
2016-09-01 20:30:07 +00:00
2015-11-23 04:50:32 +00:00
{
btScalar mass = 0 ;
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
int urdfLinkIndex = u2b . getRootLinkIndex ( ) ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , bodyHandle - > m_rootLocalInertialFrame ) ;
}
if ( m_data - > m_verboseOutput )
{
b3Printf ( " loaded %s OK! " , fileName ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btTransform tr ;
tr . setIdentity ( ) ;
tr . setOrigin ( pos ) ;
tr . setRotation ( orn ) ;
//int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation ( m_data - > m_guiHelper ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
ConvertURDF2Bullet ( u2b , creation , tr , m_data - > m_dynamicsWorld , useMultiBody , u2b . getPathPrefix ( ) ) ;
2016-06-24 14:31:17 +00:00
2016-04-11 23:42:02 +00:00
for ( int i = 0 ; i < u2b . getNumAllocatedCollisionShapes ( ) ; i + + )
{
btCollisionShape * shape = u2b . getAllocatedCollisionShape ( i ) ;
m_data - > m_collisionShapes . push_back ( shape ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btMultiBody * mb = creation . getBulletMultiBody ( ) ;
2016-08-23 01:14:29 +00:00
btRigidBody * rb = creation . getRigidBody ( ) ;
2016-09-01 20:30:07 +00:00
2015-11-23 04:50:32 +00:00
if ( useMultiBody )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( mb )
{
2016-09-01 20:30:07 +00:00
mb - > setUserIndex2 ( bodyUniqueId ) ;
2015-11-23 04:50:32 +00:00
bodyHandle - > m_multiBody = mb ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
createJointMotors ( mb ) ;
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
UrdfLinkNameMapUtil * util = new UrdfLinkNameMapUtil ;
m_data - > m_urdfLinkNameMapper . push_back ( util ) ;
util - > m_mb = mb ;
util - > m_memSerializer = new btDefaultSerializer ( bufferSizeInBytes , ( unsigned char * ) bufferServerToClient ) ;
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getBaseCollider ( ) , 0 ) ;
2016-04-29 21:45:15 +00:00
bodyHandle - > m_linkLocalInertialFrames . reserve ( mb - > getNumLinks ( ) ) ;
2015-11-23 04:50:32 +00:00
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
//disable serialization of the collision objects
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getLink ( i ) . m_collider , 0 ) ;
int urdfLinkIndex = creation . m_mb2urdfLink [ i ] ;
2016-04-29 21:45:15 +00:00
btScalar mass ;
btVector3 localInertiaDiagonal ( 0 , 0 , 0 ) ;
btTransform localInertialFrame ;
u2b . getMassAndInertia ( urdfLinkIndex , mass , localInertiaDiagonal , localInertialFrame ) ;
bodyHandle - > m_linkLocalInertialFrames . push_back ( localInertialFrame ) ;
2015-11-23 04:50:32 +00:00
std : : string * linkName = new std : : string ( u2b . getLinkName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( linkName ) ;
util - > m_memSerializer - > registerNameForPointer ( linkName - > c_str ( ) , linkName - > c_str ( ) ) ;
mb - > getLink ( i ) . m_linkName = linkName - > c_str ( ) ;
std : : string * jointName = new std : : string ( u2b . getJointName ( urdfLinkIndex ) . c_str ( ) ) ;
m_data - > m_strings . push_back ( jointName ) ;
util - > m_memSerializer - > registerNameForPointer ( jointName - > c_str ( ) , jointName - > c_str ( ) ) ;
mb - > getLink ( i ) . m_jointName = jointName - > c_str ( ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
std : : string * baseName = new std : : string ( u2b . getLinkName ( u2b . getRootLinkIndex ( ) ) ) ;
m_data - > m_strings . push_back ( baseName ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
util - > m_memSerializer - > registerNameForPointer ( baseName - > c_str ( ) , baseName - > c_str ( ) ) ;
mb - > setBaseName ( baseName - > c_str ( ) ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
util - > m_memSerializer - > insertHeader ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int len = mb - > calculateSerializeBufferSize ( ) ;
btChunk * chunk = util - > m_memSerializer - > allocate ( len , 1 ) ;
const char * structType = mb - > serialize ( chunk - > m_oldPtr , util - > m_memSerializer ) ;
util - > m_memSerializer - > finalizeChunk ( chunk , structType , BT_MULTIBODY_CODE , mb ) ;
2016-06-13 17:11:28 +00:00
return true ;
2015-11-23 04:50:32 +00:00
} else
{
b3Warning ( " No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later. " ) ;
return false ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
2016-08-23 01:14:29 +00:00
if ( rb )
{
bodyHandle - > m_rigidBody = rb ;
2016-09-01 20:30:07 +00:00
rb - > setUserIndex2 ( bodyUniqueId ) ;
2016-08-23 01:14:29 +00:00
return true ;
}
2015-11-23 04:50:32 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return false ;
}
2016-04-09 01:17:17 +00:00
void PhysicsServerCommandProcessor : : replayLogCommand ( char * bufferServerToClient , int bufferSizeInBytes )
{
if ( m_data - > m_logPlayback )
{
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
SharedMemoryCommand clientCmd ;
SharedMemoryStatus serverStatus ;
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
bool hasCommand = m_data - > m_logPlayback - > processNextCommand ( & clientCmd ) ;
if ( hasCommand )
{
processCommand ( clientCmd , serverStatus , bufferServerToClient , bufferSizeInBytes ) ;
}
}
}
2015-11-23 04:50:32 +00:00
2016-06-15 01:41:19 +00:00
int PhysicsServerCommandProcessor : : createBodyInfoStream ( int bodyUniqueId , char * bufferServerToClient , int bufferSizeInBytes )
{
int streamSizeInBytes = 0 ;
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
btMultiBody * mb = bodyHandle - > m_multiBody ;
if ( mb )
{
UrdfLinkNameMapUtil * util = new UrdfLinkNameMapUtil ;
m_data - > m_urdfLinkNameMapper . push_back ( util ) ;
util - > m_mb = mb ;
util - > m_memSerializer = new btDefaultSerializer ( bufferSizeInBytes , ( unsigned char * ) bufferServerToClient ) ;
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getBaseCollider ( ) , 0 ) ;
2016-09-27 19:13:45 +00:00
if ( mb - > getBaseName ( ) )
{
util - > m_memSerializer - > registerNameForPointer ( mb - > getBaseName ( ) , mb - > getBaseName ( ) ) ;
}
2016-06-15 01:41:19 +00:00
bodyHandle - > m_linkLocalInertialFrames . reserve ( mb - > getNumLinks ( ) ) ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
//disable serialization of the collision objects
util - > m_memSerializer - > m_skipPointers . insert ( mb - > getLink ( i ) . m_collider , 0 ) ;
util - > m_memSerializer - > registerNameForPointer ( mb - > getLink ( i ) . m_linkName , mb - > getLink ( i ) . m_linkName ) ;
util - > m_memSerializer - > registerNameForPointer ( mb - > getLink ( i ) . m_jointName , mb - > getLink ( i ) . m_jointName ) ;
}
util - > m_memSerializer - > registerNameForPointer ( mb - > getBaseName ( ) , mb - > getBaseName ( ) ) ;
2016-06-24 14:31:17 +00:00
2016-06-15 01:41:19 +00:00
util - > m_memSerializer - > insertHeader ( ) ;
int len = mb - > calculateSerializeBufferSize ( ) ;
btChunk * chunk = util - > m_memSerializer - > allocate ( len , 1 ) ;
const char * structType = mb - > serialize ( chunk - > m_oldPtr , util - > m_memSerializer ) ;
util - > m_memSerializer - > finalizeChunk ( chunk , structType , BT_MULTIBODY_CODE , mb ) ;
streamSizeInBytes = util - > m_memSerializer - > getCurrentBufferSize ( ) ;
2015-11-23 04:50:32 +00:00
2016-06-15 01:41:19 +00:00
}
return streamSizeInBytes ;
}
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : processCommand ( const struct SharedMemoryCommand & clientCmd , struct SharedMemoryStatus & serverStatusOut , char * bufferServerToClient , int bufferSizeInBytes )
{
2016-09-01 17:45:14 +00:00
2015-11-23 04:50:32 +00:00
bool hasStatus = false ;
{
///we ignore overflow of integer for now
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
2016-04-09 01:17:17 +00:00
# if 1
2015-11-23 04:50:32 +00:00
if ( m_data - > m_commandLogger )
{
2016-04-09 01:17:17 +00:00
m_data - > m_commandLogger - > logCommand ( clientCmd ) ;
2015-11-23 04:50:32 +00:00
}
# endif
//m_data->m_testBlock1->m_numProcessedClientCommands++;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//no timestamp yet
int timeStamp = 0 ;
2016-09-08 22:15:58 +00:00
//catch uninitialized cases
serverStatusOut . m_type = CMD_INVALID_STATUS ;
2015-11-23 04:50:32 +00:00
//consume the command
switch ( clientCmd . m_type )
{
#if 0
case CMD_SEND_BULLET_DATA_STREAM :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_SEND_BULLET_DATA_STREAM length %d " , clientCmd . m_dataStreamArguments . m_streamChunkLength ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btBulletWorldImporter * worldImporter = new btBulletWorldImporter ( m_data - > m_dynamicsWorld ) ;
m_data - > m_worldImporters . push_back ( worldImporter ) ;
bool completedOk = worldImporter - > loadFileFromMemory ( m_data - > m_testBlock1 - > m_bulletStreamDataClientToServer , clientCmd . m_dataStreamArguments . m_streamChunkLength ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( completedOk )
{
SharedMemoryStatus & status = m_data - > createServerStatus ( CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED , clientCmd . m_sequenceNumber , timeStamp ) ;
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
m_data - > submitServerStatus ( status ) ;
} else
{
SharedMemoryStatus & status = m_data - > createServerStatus ( CMD_BULLET_DATA_STREAM_RECEIVED_FAILED , clientCmd . m_sequenceNumber , timeStamp ) ;
m_data - > submitServerStatus ( status ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
# endif
case CMD_REQUEST_DEBUG_LINES :
{
int curFlags = m_data - > m_remoteDebugDrawer - > getDebugMode ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int debugMode = clientCmd . m_requestDebugLinesArguments . m_debugMode ; //clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
int startingLineIndex = clientCmd . m_requestDebugLinesArguments . m_startingLineIndex ;
if ( startingLineIndex < 0 )
{
b3Warning ( " startingLineIndex should be non-negative " ) ;
startingLineIndex = 0 ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( clientCmd . m_requestDebugLinesArguments . m_startingLineIndex = = 0 )
{
m_data - > m_remoteDebugDrawer - > m_lines2 . resize ( 0 ) ;
//|btIDebugDraw::DBG_DrawAabb|
// btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ;
m_data - > m_remoteDebugDrawer - > setDebugMode ( debugMode ) ;
btIDebugDraw * oldDebugDrawer = m_data - > m_dynamicsWorld - > getDebugDrawer ( ) ;
m_data - > m_dynamicsWorld - > setDebugDrawer ( m_data - > m_remoteDebugDrawer ) ;
m_data - > m_dynamicsWorld - > debugDrawWorld ( ) ;
m_data - > m_dynamicsWorld - > setDebugDrawer ( oldDebugDrawer ) ;
m_data - > m_remoteDebugDrawer - > setDebugMode ( curFlags ) ;
}
//9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color'
int maxNumLines = bufferSizeInBytes / ( sizeof ( float ) * 9 ) - 1 ;
if ( startingLineIndex > m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) )
{
b3Warning ( " m_startingLineIndex exceeds total number of debug lines " ) ;
startingLineIndex = m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int numLines = btMin ( maxNumLines , m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) - startingLineIndex ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( numLines )
{
float * linesFrom = ( float * ) bufferServerToClient ;
float * linesTo = ( float * ) ( bufferServerToClient + numLines * 3 * sizeof ( float ) ) ;
float * linesColor = ( float * ) ( bufferServerToClient + 2 * numLines * 3 * sizeof ( float ) ) ;
for ( int i = 0 ; i < numLines ; i + + )
{
linesFrom [ i * 3 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_from . x ( ) ;
linesTo [ i * 3 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_to . x ( ) ;
linesColor [ i * 3 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_color . x ( ) ;
linesFrom [ i * 3 + 1 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_from . y ( ) ;
linesTo [ i * 3 + 1 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_to . y ( ) ;
linesColor [ i * 3 + 1 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_color . y ( ) ;
linesFrom [ i * 3 + 2 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_from . z ( ) ;
linesTo [ i * 3 + 2 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_to . z ( ) ;
linesColor [ i * 3 + 2 ] = m_data - > m_remoteDebugDrawer - > m_lines2 [ i + startingLineIndex ] . m_color . z ( ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_DEBUG_LINES_COMPLETED ;
serverStatusOut . m_sendDebugLinesArgs . m_numDebugLines = numLines ;
serverStatusOut . m_sendDebugLinesArgs . m_startingLineIndex = startingLineIndex ;
serverStatusOut . m_sendDebugLinesArgs . m_numRemainingDebugLines = m_data - > m_remoteDebugDrawer - > m_lines2 . size ( ) - ( startingLineIndex + numLines ) ;
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
2016-05-18 06:57:19 +00:00
case CMD_REQUEST_CAMERA_IMAGE_DATA :
{
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
int startPixelIndex = clientCmd . m_requestPixelDataArguments . m_startPixelIndex ;
2016-07-13 01:16:13 +00:00
int width = clientCmd . m_requestPixelDataArguments . m_pixelWidth ;
int height = clientCmd . m_requestPixelDataArguments . m_pixelHeight ;
2016-05-31 17:23:04 +00:00
int numPixelsCopied = 0 ;
2016-06-24 14:31:17 +00:00
2016-09-01 17:45:14 +00:00
2016-06-24 14:31:17 +00:00
2016-07-13 01:16:13 +00:00
if ( ( clientCmd . m_updateFlags & ER_BULLET_HARDWARE_OPENGL ) ! = 0 )
2016-06-01 05:55:13 +00:00
{
2016-07-13 01:16:13 +00:00
//m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,0,0,0,0,0,width,height,0);
2016-06-24 14:31:17 +00:00
}
2016-06-02 00:47:41 +00:00
else
2016-06-01 18:04:10 +00:00
{
2016-07-13 01:16:13 +00:00
if ( ( clientCmd . m_requestPixelDataArguments . m_startPixelIndex = = 0 ) & &
( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT ) ! = 0 )
{
m_data - > m_visualConverter . setWidthAndHeight ( clientCmd . m_requestPixelDataArguments . m_pixelWidth ,
clientCmd . m_requestPixelDataArguments . m_pixelHeight ) ;
}
2016-06-02 00:47:41 +00:00
m_data - > m_visualConverter . getWidthAndHeight ( width , height ) ;
2016-06-01 05:55:13 +00:00
}
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
int numTotalPixels = width * height ;
int numRemainingPixels = numTotalPixels - startPixelIndex ;
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
if ( numRemainingPixels > 0 )
{
2016-08-11 21:55:30 +00:00
int totalBytesPerPixel = 4 + 4 + 4 ; //4 for rgb, 4 for depth, 4 for segmentation mask
int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1 ;
2016-05-31 17:23:04 +00:00
unsigned char * pixelRGBA = ( unsigned char * ) bufferServerToClient ;
int numRequestedPixels = btMin ( maxNumPixels , numRemainingPixels ) ;
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
float * depthBuffer = ( float * ) ( bufferServerToClient + numRequestedPixels * 4 ) ;
2016-08-11 21:55:30 +00:00
int * segmentationMaskBuffer = ( int * ) ( bufferServerToClient + numRequestedPixels * 8 ) ;
2016-06-24 14:31:17 +00:00
2016-07-13 01:16:13 +00:00
if ( ( clientCmd . m_updateFlags & ER_BULLET_HARDWARE_OPENGL ) ! = 0 )
2016-06-01 18:04:10 +00:00
{
2016-08-11 21:55:30 +00:00
m_data - > m_guiHelper - > copyCameraImageData ( clientCmd . m_requestPixelDataArguments . m_viewMatrix ,
clientCmd . m_requestPixelDataArguments . m_projectionMatrix , pixelRGBA , numRequestedPixels ,
depthBuffer , numRequestedPixels ,
segmentationMaskBuffer , numRequestedPixels ,
startPixelIndex , width , height , & numPixelsCopied ) ;
2016-06-01 18:04:10 +00:00
} else
{
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
if ( clientCmd . m_requestPixelDataArguments . m_startPixelIndex = = 0 )
{
2016-06-07 01:54:05 +00:00
// printf("-------------------------------\nRendering\n");
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
if ( ( clientCmd . m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ) ! = 0 )
{
m_data - > m_visualConverter . render (
clientCmd . m_requestPixelDataArguments . m_viewMatrix ,
clientCmd . m_requestPixelDataArguments . m_projectionMatrix ) ;
} else
{
m_data - > m_visualConverter . render ( ) ;
}
2016-06-24 14:31:17 +00:00
2016-06-02 00:47:41 +00:00
}
2016-06-24 14:31:17 +00:00
2016-08-11 21:55:30 +00:00
m_data - > m_visualConverter . copyCameraImageData ( pixelRGBA , numRequestedPixels ,
depthBuffer , numRequestedPixels ,
segmentationMaskBuffer , numRequestedPixels ,
startPixelIndex , & width , & height , & numPixelsCopied ) ;
2016-06-01 18:04:10 +00:00
}
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
//each pixel takes 4 RGBA values and 1 float = 8 bytes
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
} else
{
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
}
2016-06-24 14:31:17 +00:00
2016-05-31 17:23:04 +00:00
serverStatusOut . m_type = CMD_CAMERA_IMAGE_COMPLETED ;
serverStatusOut . m_sendPixelDataArguments . m_numPixelsCopied = numPixelsCopied ;
serverStatusOut . m_sendPixelDataArguments . m_numRemainingPixels = numRemainingPixels - numPixelsCopied ;
serverStatusOut . m_sendPixelDataArguments . m_startingPixelIndex = startPixelIndex ;
serverStatusOut . m_sendPixelDataArguments . m_imageWidth = width ;
serverStatusOut . m_sendPixelDataArguments . m_imageHeight = height ;
2016-05-18 06:57:19 +00:00
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2016-05-18 06:57:19 +00:00
break ;
}
2016-06-15 01:41:19 +00:00
case CMD_REQUEST_BODY_INFO :
{
const SdfRequestInfoArgs & sdfInfoArgs = clientCmd . m_sdfRequestInfoArgs ;
//stream info into memory
int streamSizeInBytes = createBodyInfoStream ( sdfInfoArgs . m_bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2016-06-24 14:31:17 +00:00
2016-06-15 01:41:19 +00:00
serverStatusOut . m_type = CMD_BODY_INFO_COMPLETED ;
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = sdfInfoArgs . m_bodyUniqueId ;
serverStatusOut . m_dataStreamArguments . m_streamChunkLength = streamSizeInBytes ;
hasStatus = true ;
break ;
}
2016-10-13 06:03:36 +00:00
case CMD_SAVE_WORLD :
{
///this is a very rudimentary way to save the state of the world, for scene authoring
///many todo's, for example save the state of motor controllers etc.
2016-10-14 22:06:09 +00:00
2016-10-13 06:03:36 +00:00
{
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
FILE * f = fopen ( clientCmd . m_sdfArguments . m_sdfFileName , " w " ) ;
if ( f )
{
char line [ 1024 ] ;
{
sprintf ( line , " import pybullet as p \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
{
sprintf ( line , " p.connect(p.SHARED_MEMORY) \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
//for each objects ...
for ( int i = 0 ; i < m_data - > m_saveWorldBodyData . size ( ) ; i + + )
{
SaveWorldObjectData & sd = m_data - > m_saveWorldBodyData [ i ] ;
for ( int i = 0 ; i < sd . m_bodyUniqueIds . size ( ) ; i + + )
{
{
int bodyUniqueId = sd . m_bodyUniqueIds [ i ] ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body )
{
if ( body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
btTransform comTr = mb - > getBaseWorldTransform ( ) ;
btTransform tr = comTr * body - > m_rootLocalInertialFrame . inverse ( ) ;
if ( strstr ( sd . m_fileName . c_str ( ) , " .urdf " ) )
{
sprintf ( line , " objects = [p.loadURDF( \" %s \" , %f,%f,%f,%f,%f,%f,%f)] \n " , sd . m_fileName . c_str ( ) ,
tr . getOrigin ( ) [ 0 ] , tr . getOrigin ( ) [ 1 ] , tr . getOrigin ( ) [ 2 ] ,
tr . getRotation ( ) [ 0 ] , tr . getRotation ( ) [ 1 ] , tr . getRotation ( ) [ 2 ] , tr . getRotation ( ) [ 3 ] ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
if ( strstr ( sd . m_fileName . c_str ( ) , " .sdf " ) & & i = = 0 )
{
sprintf ( line , " objects = p.loadSDF( \" %s \" ) \n " , sd . m_fileName . c_str ( ) ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
if ( strstr ( sd . m_fileName . c_str ( ) , " .sdf " ) | | ( ( strstr ( sd . m_fileName . c_str ( ) , " .urdf " ) ) & & mb - > getNumLinks ( ) ) )
{
sprintf ( line , " ob = objects[%d] \n " , i ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
if ( strstr ( sd . m_fileName . c_str ( ) , " .sdf " ) )
{
sprintf ( line , " p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f]) \n " ,
comTr . getOrigin ( ) [ 0 ] , comTr . getOrigin ( ) [ 1 ] , comTr . getOrigin ( ) [ 2 ] ,
comTr . getRotation ( ) [ 0 ] , comTr . getRotation ( ) [ 1 ] , comTr . getRotation ( ) [ 2 ] , comTr . getRotation ( ) [ 3 ] ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
if ( mb - > getNumLinks ( ) )
{
{
sprintf ( line , " jointPositions=[ " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
btScalar jointPos = mb - > getJointPosMultiDof ( i ) [ 0 ] ;
if ( i < mb - > getNumLinks ( ) - 1 )
{
sprintf ( line , " %f, " , jointPos ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
} else
{
sprintf ( line , " %f " , jointPos ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
}
{
sprintf ( line , " ] \n for jointIndex in range (p.getNumJoints(ob)): \n \t p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) \n \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
}
} else
{
//todo: btRigidBody/btSoftBody etc case
}
}
}
}
//for URDF, load at origin, then reposition...
struct SaveWorldObjectData
{
b3AlignedObjectArray < int > m_bodyUniqueIds ;
std : : string m_fileName ;
} ;
}
{
btVector3 grav = this - > m_data - > m_dynamicsWorld - > getGravity ( ) ;
sprintf ( line , " p.setGravity(%f,%f,%f) \n " , grav [ 0 ] , grav [ 1 ] , grav [ 2 ] ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
{
sprintf ( line , " p.stepSimulation() \n p.disconnect() \n " ) ;
int len = strlen ( line ) ;
fwrite ( line , len , 1 , f ) ;
}
fclose ( f ) ;
}
serverStatusOut . m_type = CMD_SAVE_WORLD_COMPLETED ;
hasStatus = true ;
break ;
}
serverStatusOut . m_type = CMD_SAVE_WORLD_FAILED ;
hasStatus = true ;
break ;
}
2016-06-13 17:11:28 +00:00
case CMD_LOAD_SDF :
{
const SdfArgs & sdfArgs = clientCmd . m_sdfArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_SDF:%s " , sdfArgs . m_sdfFileName ) ;
}
2016-08-17 00:56:30 +00:00
bool useMultiBody = ( clientCmd . m_updateFlags & URDF_ARGS_USE_MULTIBODY ) ? sdfArgs . m_useMultiBody : true ;
2016-06-24 14:31:17 +00:00
2016-08-17 00:56:30 +00:00
bool completedOk = loadSdf ( sdfArgs . m_sdfFileName , bufferServerToClient , bufferSizeInBytes , useMultiBody ) ;
2016-07-25 18:48:44 +00:00
if ( completedOk )
{
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
serverStatusOut . m_sdfLoadedArgs . m_numBodies = m_data - > m_sdfRecentLoadedBodies . size ( ) ;
int maxBodies = btMin ( MAX_SDF_BODIES , serverStatusOut . m_sdfLoadedArgs . m_numBodies ) ;
for ( int i = 0 ; i < maxBodies ; i + + )
{
serverStatusOut . m_sdfLoadedArgs . m_bodyUniqueIds [ i ] = m_data - > m_sdfRecentLoadedBodies [ i ] ;
}
2016-06-24 14:31:17 +00:00
2016-07-25 18:48:44 +00:00
serverStatusOut . m_type = CMD_SDF_LOADING_COMPLETED ;
} else
2016-06-13 17:11:28 +00:00
{
2016-07-25 18:48:44 +00:00
serverStatusOut . m_type = CMD_SDF_LOADING_FAILED ;
2016-06-13 17:11:28 +00:00
}
hasStatus = true ;
break ;
}
2015-11-23 04:50:32 +00:00
case CMD_LOAD_URDF :
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
const UrdfArgs & urdfArgs = clientCmd . m_urdfArguments ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_LOAD_URDF:%s " , urdfArgs . m_urdfFileName ) ;
}
btAssert ( ( clientCmd . m_updateFlags & URDF_ARGS_FILE_NAME ) ! = 0 ) ;
btAssert ( urdfArgs . m_urdfFileName ) ;
btVector3 initialPos ( 0 , 0 , 0 ) ;
btQuaternion initialOrn ( 0 , 0 , 0 , 1 ) ;
if ( clientCmd . m_updateFlags & URDF_ARGS_INITIAL_POSITION )
{
initialPos [ 0 ] = urdfArgs . m_initialPosition [ 0 ] ;
initialPos [ 1 ] = urdfArgs . m_initialPosition [ 1 ] ;
initialPos [ 2 ] = urdfArgs . m_initialPosition [ 2 ] ;
}
if ( clientCmd . m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION )
{
initialOrn [ 0 ] = urdfArgs . m_initialOrientation [ 0 ] ;
initialOrn [ 1 ] = urdfArgs . m_initialOrientation [ 1 ] ;
initialOrn [ 2 ] = urdfArgs . m_initialOrientation [ 2 ] ;
initialOrn [ 3 ] = urdfArgs . m_initialOrientation [ 3 ] ;
}
bool useMultiBody = ( clientCmd . m_updateFlags & URDF_ARGS_USE_MULTIBODY ) ? urdfArgs . m_useMultiBody : true ;
bool useFixedBase = ( clientCmd . m_updateFlags & URDF_ARGS_USE_FIXED_BASE ) ? urdfArgs . m_useFixedBase : false ;
int bodyUniqueId ;
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf ( urdfArgs . m_urdfFileName ,
initialPos , initialOrn ,
useMultiBody , useFixedBase , & bodyUniqueId , bufferServerToClient , bufferSizeInBytes ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( completedOk )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_guiHelper - > autogenerateGraphicsObjects ( this - > m_data - > m_dynamicsWorld ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_URDF_LOADING_COMPLETED ;
2016-04-11 23:42:02 +00:00
serverStatusOut . m_dataStreamArguments . m_streamChunkLength = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_urdfLinkNameMapper . size ( ) )
{
serverStatusOut . m_dataStreamArguments . m_streamChunkLength = m_data - > m_urdfLinkNameMapper . at ( m_data - > m_urdfLinkNameMapper . size ( ) - 1 ) - > m_memSerializer - > getCurrentBufferSize ( ) ;
}
serverStatusOut . m_dataStreamArguments . m_bodyUniqueId = bodyUniqueId ;
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
serverStatusOut . m_type = CMD_URDF_LOADING_FAILED ;
hasStatus = true ;
}
2016-06-24 14:31:17 +00:00
2016-10-17 20:01:04 +00:00
break ;
}
case CMD_LOAD_BUNNY :
{
2016-10-19 00:38:43 +00:00
# ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-17 20:01:04 +00:00
m_data - > m_softBodyWorldInfo . air_density = ( btScalar ) 1.2 ;
m_data - > m_softBodyWorldInfo . water_density = 0 ;
m_data - > m_softBodyWorldInfo . water_offset = 0 ;
m_data - > m_softBodyWorldInfo . water_normal = btVector3 ( 0 , 0 , 0 ) ;
m_data - > m_softBodyWorldInfo . m_gravity . setValue ( 0 , 0 , - 10 ) ;
m_data - > m_softBodyWorldInfo . m_broadphase = m_data - > m_broadphase ;
m_data - > m_softBodyWorldInfo . m_sparsesdf . Initialize ( ) ;
btSoftBody * psb = btSoftBodyHelpers : : CreateFromTriMesh ( m_data - > m_softBodyWorldInfo , gVerticesBunny , & gIndicesBunny [ 0 ] [ 0 ] , BUNNY_NUM_TRIANGLES ) ;
btSoftBody : : Material * pm = psb - > appendMaterial ( ) ;
pm - > m_kLST = 1.0 ;
pm - > m_flags - = btSoftBody : : fMaterial : : DebugDraw ;
psb - > generateBendingConstraints ( 2 , pm ) ;
psb - > m_cfg . piterations = 2 ;
psb - > m_cfg . kDF = 0.5 ;
psb - > randomizeConstraints ( ) ;
psb - > rotate ( btQuaternion ( 0.70711 , 0 , 0 , 0.70711 ) ) ;
psb - > translate ( btVector3 ( 0 , 0 , 3.0 ) ) ;
psb - > scale ( btVector3 ( 0.1 , 0.1 , 0.1 ) ) ;
psb - > setTotalMass ( 1 , true ) ;
psb - > getCollisionShape ( ) - > setMargin ( 0.01 ) ;
m_data - > m_dynamicsWorld - > addSoftBody ( psb ) ;
2016-10-19 00:38:43 +00:00
# endif
2015-11-23 04:50:32 +00:00
break ;
}
case CMD_CREATE_SENSOR :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_CREATE_SENSOR " ) ;
}
int bodyUniqueId = clientCmd . m_createSensorArguments . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
btAssert ( mb ) ;
for ( int i = 0 ; i < clientCmd . m_createSensorArguments . m_numJointSensorChanges ; i + + )
{
int jointIndex = clientCmd . m_createSensorArguments . m_jointIndex [ i ] ;
if ( clientCmd . m_createSensorArguments . m_enableJointForceSensor [ i ] )
{
if ( mb - > getLink ( jointIndex ) . m_jointFeedback )
{
b3Warning ( " CMD_CREATE_SENSOR: sensor for joint [%d] already enabled " , jointIndex ) ;
} else
{
btMultiBodyJointFeedback * fb = new btMultiBodyJointFeedback ( ) ;
fb - > m_reactionForces . setZero ( ) ;
mb - > getLink ( jointIndex ) . m_jointFeedback = fb ;
m_data - > m_multiBodyJointFeedbacks . push_back ( fb ) ;
} ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
if ( mb - > getLink ( jointIndex ) . m_jointFeedback )
{
m_data - > m_multiBodyJointFeedbacks . remove ( mb - > getLink ( jointIndex ) . m_jointFeedback ) ;
delete mb - > getLink ( jointIndex ) . m_jointFeedback ;
mb - > getLink ( jointIndex ) . m_jointFeedback = 0 ;
} else
{
b3Warning ( " CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d] " , jointIndex ) ;
} ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
b3Warning ( " No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet " ) ;
}
#if 0
//todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody
/*
for ( int i = 0 ; i < m_data - > m_dynamicsWorld - > getNumConstraints ( ) ; i + + )
{
btTypedConstraint * c = m_data - > m_dynamicsWorld - > getConstraint ( i ) ;
btJointFeedback * fb = new btJointFeedback ( ) ;
m_data - > m_jointFeedbacks . push_back ( fb ) ;
c - > setJointFeedback ( fb ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
*/
# endif
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverStatusOut . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
break ;
}
2016-10-05 23:31:48 +00:00
case CMD_SEND_DESIRED_STATE :
2015-11-23 04:50:32 +00:00
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Processed CMD_SEND_DESIRED_STATE " ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int bodyUniqueId = clientCmd . m_sendDesiredStateCommandArgument . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
btAssert ( mb ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
switch ( clientCmd . m_sendDesiredStateCommandArgument . m_controlMode )
{
case CONTROL_MODE_TORQUE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_TORQUE " ) ;
}
2016-06-24 14:31:17 +00:00
// mb->clearForcesAndTorques();
2016-08-18 22:36:18 +00:00
int torqueIndex = 6 ;
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
{
for ( int dof = 0 ; dof < mb - > getLink ( link ) . m_dofCount ; dof + + )
{
double torque = 0.f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ torqueIndex ] & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
{
torque = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ torqueIndex ] ;
mb - > addJointTorqueMultiDof ( link , dof , torque ) ;
}
torqueIndex + + ;
}
2015-11-23 04:50:32 +00:00
}
}
break ;
}
case CONTROL_MODE_VELOCITY :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_VELOCITY " ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int dofIndex = 6 ; //skip the 3 linear + 3 angular degree of freedom entries of the base
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
{
if ( supportsJointMotor ( mb , link ) )
{
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
if ( motor )
2015-11-23 04:50:32 +00:00
{
2016-06-04 02:03:56 +00:00
btScalar desiredVelocity = 0.f ;
2016-06-24 14:31:17 +00:00
bool hasDesiredVelocity = false ;
2015-11-23 04:50:32 +00:00
2016-09-28 18:17:11 +00:00
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
{
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ dofIndex ] ;
2016-06-24 18:06:56 +00:00
btScalar kd = 0.1f ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
{
kd = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ dofIndex ] ;
2016-09-01 17:45:14 +00:00
}
2016-06-24 18:06:56 +00:00
motor - > setVelocityTarget ( desiredVelocity , kd ) ;
2016-09-01 17:45:14 +00:00
2016-06-24 18:06:56 +00:00
btScalar kp = 0.f ;
motor - > setPositionTarget ( 0 , kp ) ;
2016-06-24 14:31:17 +00:00
hasDesiredVelocity = true ;
}
if ( hasDesiredVelocity )
{
btScalar maxImp = 1000000.f * m_data - > m_physicsDeltaTime ;
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ dofIndex ] & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
{
maxImp = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ dofIndex ] * m_data - > m_physicsDeltaTime ;
}
motor - > setMaxAppliedImpulse ( maxImp ) ;
}
2015-11-23 04:50:32 +00:00
numMotors + + ;
}
}
dofIndex + = mb - > getLink ( link ) . m_dofCount ;
}
}
break ;
}
2016-06-04 02:03:56 +00:00
2015-11-23 04:50:32 +00:00
case CONTROL_MODE_POSITION_VELOCITY_PD :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Using CONTROL_MODE_POSITION_VELOCITY_PD " ) ;
}
//compute the force base on PD control
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int velIndex = 6 ; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7 ; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
{
if ( supportsJointMotor ( mb , link ) )
{
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
if ( motor )
2015-11-23 04:50:32 +00:00
{
2016-06-24 14:31:17 +00:00
bool hasDesiredPosOrVel = false ;
btScalar kp = 0.f ;
btScalar kd = 0.f ;
2016-06-04 02:03:56 +00:00
btScalar desiredVelocity = 0.f ;
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_QDOT ) ! = 0 )
{
hasDesiredPosOrVel = true ;
2016-06-04 02:03:56 +00:00
desiredVelocity = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ velIndex ] ;
2016-06-24 14:31:17 +00:00
kd = 0.1 ;
}
2016-06-04 02:03:56 +00:00
btScalar desiredPosition = 0.f ;
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ posIndex ] & SIM_DESIRED_STATE_HAS_Q ) ! = 0 )
{
hasDesiredPosOrVel = true ;
2016-06-04 02:03:56 +00:00
desiredPosition = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateQ [ posIndex ] ;
2016-06-24 14:31:17 +00:00
kp = 0.1 ;
}
2016-06-04 02:03:56 +00:00
2016-06-24 14:31:17 +00:00
if ( hasDesiredPosOrVel )
{
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KP ) ! = 0 )
{
kp = clientCmd . m_sendDesiredStateCommandArgument . m_Kp [ velIndex ] ;
}
if ( ( clientCmd . m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ velIndex ] & SIM_DESIRED_STATE_HAS_KD ) ! = 0 )
{
kd = clientCmd . m_sendDesiredStateCommandArgument . m_Kd [ velIndex ] ;
}
2016-06-24 18:06:56 +00:00
motor - > setVelocityTarget ( desiredVelocity , kd ) ;
motor - > setPositionTarget ( desiredPosition , kp ) ;
2016-09-01 17:45:14 +00:00
2016-06-24 14:31:17 +00:00
btScalar maxImp = 1000000.f * m_data - > m_physicsDeltaTime ;
if ( ( clientCmd . m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE ) ! = 0 )
maxImp = clientCmd . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ velIndex ] * m_data - > m_physicsDeltaTime ;
motor - > setMaxAppliedImpulse ( maxImp ) ;
}
2015-11-23 04:50:32 +00:00
numMotors + + ;
}
}
velIndex + = mb - > getLink ( link ) . m_dofCount ;
posIndex + = mb - > getLink ( link ) . m_posVarCount ;
}
}
break ;
}
default :
2016-10-05 23:31:48 +00:00
{
b3Warning ( " m_controlMode not implemented yet " ) ;
break ;
}
2016-06-24 14:31:17 +00:00
2016-10-05 23:31:48 +00:00
}
}
2016-06-24 14:31:17 +00:00
2016-10-05 23:31:48 +00:00
serverStatusOut . m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED ;
hasStatus = true ;
break ;
}
2015-11-23 04:50:32 +00:00
case CMD_REQUEST_ACTUAL_STATE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Sending the actual state (Q,U) " ) ;
}
int bodyUniqueId = clientCmd . m_requestActualStateInformationCommandArgument . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverStatusOut . m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED ;
serverCmd . m_sendActualStateArgs . m_bodyUniqueId = bodyUniqueId ;
int totalDegreeOfFreedomQ = 0 ;
2016-06-24 14:31:17 +00:00
int totalDegreeOfFreedomU = 0 ;
2016-08-18 02:35:52 +00:00
if ( mb - > getNumLinks ( ) > = MAX_DEGREE_OF_FREEDOM )
{
serverStatusOut . m_type = CMD_ACTUAL_STATE_UPDATE_FAILED ;
hasStatus = true ;
break ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
btTransform tr ;
tr . setOrigin ( mb - > getBasePos ( ) ) ;
tr . setRotation ( mb - > getWorldToBaseRot ( ) . inverse ( ) ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 0 ] =
body - > m_rootLocalInertialFrame . getOrigin ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 1 ] =
body - > m_rootLocalInertialFrame . getOrigin ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 2 ] =
body - > m_rootLocalInertialFrame . getOrigin ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 3 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 4 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 5 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_rootLocalInertialFrame [ 6 ] =
body - > m_rootLocalInertialFrame . getRotation ( ) [ 3 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//base position in world space, carthesian
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 0 ] = tr . getOrigin ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 1 ] = tr . getOrigin ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 2 ] = tr . getOrigin ( ) [ 2 ] ;
//base orientation, quaternion x,y,z,w, in world space, carthesian
2016-06-24 14:31:17 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 3 ] = tr . getRotation ( ) [ 0 ] ;
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 4 ] = tr . getRotation ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 5 ] = tr . getRotation ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 6 ] = tr . getRotation ( ) [ 3 ] ;
totalDegreeOfFreedomQ + = 7 ; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 0 ] = mb - > getBaseVel ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 1 ] = mb - > getBaseVel ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 2 ] = mb - > getBaseVel ( ) [ 2 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//base angular velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 3 ] = mb - > getBaseOmega ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 4 ] = mb - > getBaseOmega ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 5 ] = mb - > getBaseOmega ( ) [ 2 ] ;
totalDegreeOfFreedomU + = 6 ; //3 linear and 3 angular DOF
}
for ( int l = 0 ; l < mb - > getNumLinks ( ) ; l + + )
{
for ( int d = 0 ; d < mb - > getLink ( l ) . m_posVarCount ; d + + )
{
serverCmd . m_sendActualStateArgs . m_actualStateQ [ totalDegreeOfFreedomQ + + ] = mb - > getJointPosMultiDof ( l ) [ d ] ;
}
for ( int d = 0 ; d < mb - > getLink ( l ) . m_dofCount ; d + + )
{
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ totalDegreeOfFreedomU + + ] = mb - > getJointVelMultiDof ( l ) [ d ] ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( 0 = = mb - > getLink ( l ) . m_jointFeedback )
{
for ( int d = 0 ; d < 6 ; d + + )
{
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + d ] = 0 ;
}
} else
{
btVector3 sensedForce = mb - > getLink ( l ) . m_jointFeedback - > m_reactionForces . getLinear ( ) ;
btVector3 sensedTorque = mb - > getLink ( l ) . m_jointFeedback - > m_reactionForces . getAngular ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 0 ] = sensedForce [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 1 ] = sensedForce [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 2 ] = sensedForce [ 2 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 3 ] = sensedTorque [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 4 ] = sensedTorque [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_jointReactionForces [ l * 6 + 5 ] = sensedTorque [ 2 ] ;
}
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
serverCmd . m_sendActualStateArgs . m_jointMotorForce [ l ] = 0 ;
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
if ( supportsJointMotor ( mb , l ) )
{
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) body - > m_multiBody - > getLink ( l ) . m_userPtr ;
2016-06-24 14:31:17 +00:00
2016-06-17 01:46:34 +00:00
if ( motor & & m_data - > m_physicsDeltaTime > btScalar ( 0 ) )
2016-04-09 01:17:17 +00:00
{
btScalar force = motor - > getAppliedImpulse ( 0 ) / m_data - > m_physicsDeltaTime ;
serverCmd . m_sendActualStateArgs . m_jointMotorForce [ l ] =
force ;
//if (force>0)
//{
// b3Printf("force = %f\n", force);
//}
}
}
2016-04-29 21:45:15 +00:00
btVector3 linkLocalInertialOrigin = body - > m_linkLocalInertialFrames [ l ] . getOrigin ( ) ;
btQuaternion linkLocalInertialRotation = body - > m_linkLocalInertialFrames [ l ] . getRotation ( ) ;
2016-06-24 14:31:17 +00:00
2016-10-12 16:57:14 +00:00
btVector3 linkCOMOrigin = mb - > getLink ( l ) . m_cachedWorldTransform . getOrigin ( ) ;
btQuaternion linkCOMRotation = mb - > getLink ( l ) . m_cachedWorldTransform . getRotation ( ) ;
2016-06-24 14:31:17 +00:00
2016-10-12 16:57:14 +00:00
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 0 ] = linkCOMOrigin . getX ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 1 ] = linkCOMOrigin . getY ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 2 ] = linkCOMOrigin . getZ ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 3 ] = linkCOMRotation . x ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 4 ] = linkCOMRotation . y ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 5 ] = linkCOMRotation . z ( ) ;
serverCmd . m_sendActualStateArgs . m_linkState [ l * 7 + 6 ] = linkCOMRotation . w ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-29 21:45:15 +00:00
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 0 ] = linkLocalInertialOrigin . getX ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 1 ] = linkLocalInertialOrigin . getY ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 2 ] = linkLocalInertialOrigin . getZ ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-29 21:45:15 +00:00
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 3 ] = linkLocalInertialRotation . x ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 4 ] = linkLocalInertialRotation . y ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 5 ] = linkLocalInertialRotation . z ( ) ;
serverCmd . m_sendActualStateArgs . m_linkLocalInertialFrames [ l * 7 + 6 ] = linkLocalInertialRotation . w ( ) ;
2016-06-24 14:31:17 +00:00
2016-04-09 01:17:17 +00:00
}
2015-11-23 04:50:32 +00:00
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ ;
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomU = totalDegreeOfFreedomU ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
hasStatus = true ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} else
{
if ( body & & body - > m_rigidBody )
{
btRigidBody * rb = body - > m_rigidBody ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED ;
serverCmd . m_sendActualStateArgs . m_bodyUniqueId = bodyUniqueId ;
int totalDegreeOfFreedomQ = 0 ;
int totalDegreeOfFreedomU = 0 ;
btTransform tr = rb - > getWorldTransform ( ) ;
//base position in world space, carthesian
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 0 ] = tr . getOrigin ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 1 ] = tr . getOrigin ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 2 ] = tr . getOrigin ( ) [ 2 ] ;
//base orientation, quaternion x,y,z,w, in world space, carthesian
2016-06-24 14:31:17 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 3 ] = tr . getRotation ( ) [ 0 ] ;
2015-11-23 04:50:32 +00:00
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 4 ] = tr . getRotation ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 5 ] = tr . getRotation ( ) [ 2 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQ [ 6 ] = tr . getRotation ( ) [ 3 ] ;
totalDegreeOfFreedomQ + = 7 ; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 0 ] = rb - > getLinearVelocity ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 1 ] = rb - > getLinearVelocity ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 2 ] = rb - > getLinearVelocity ( ) [ 2 ] ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
//base angular velocity (in world space, carthesian)
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 3 ] = rb - > getAngularVelocity ( ) [ 0 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 4 ] = rb - > getAngularVelocity ( ) [ 1 ] ;
serverCmd . m_sendActualStateArgs . m_actualStateQdot [ 5 ] = rb - > getAngularVelocity ( ) [ 2 ] ;
totalDegreeOfFreedomU + = 6 ; //3 linear and 3 angular DOF
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ ;
serverCmd . m_sendActualStateArgs . m_numDegreeOfFreedomU = totalDegreeOfFreedomU ;
hasStatus = true ;
} else
{
b3Warning ( " Request state but no multibody or rigid body available " ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_ACTUAL_STATE_UPDATE_FAILED ;
hasStatus = true ;
}
}
break ;
}
case CMD_STEP_FORWARD_SIMULATION :
{
2016-09-01 17:45:14 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Step simulation request " ) ;
2016-07-08 02:24:44 +00:00
b3Printf ( " CMD_STEP_FORWARD_SIMULATION clientCmd = %d \n " , clientCmd . m_sequenceNumber ) ;
2015-11-23 04:50:32 +00:00
}
2016-04-14 00:21:43 +00:00
///todo(erwincoumans) move this damping inside Bullet
for ( int i = 0 ; i < m_data - > m_bodyHandles . size ( ) ; i + + )
{
applyJointDamping ( i ) ;
}
2016-09-15 23:57:00 +00:00
2016-09-24 18:25:05 +00:00
2016-09-22 15:50:28 +00:00
btScalar deltaTimeScaled = m_data - > m_physicsDeltaTime * simTimeScalingFactor ;
2016-09-15 23:57:00 +00:00
if ( m_data - > m_numSimulationSubSteps > 0 )
{
2016-09-22 15:50:28 +00:00
m_data - > m_dynamicsWorld - > stepSimulation ( deltaTimeScaled , m_data - > m_numSimulationSubSteps , m_data - > m_physicsDeltaTime / m_data - > m_numSimulationSubSteps ) ;
2016-09-15 23:57:00 +00:00
}
else
{
2016-09-22 15:50:28 +00:00
m_data - > m_dynamicsWorld - > stepSimulation ( deltaTimeScaled , 0 ) ;
2016-09-15 23:57:00 +00:00
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED ;
hasStatus = true ;
break ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS :
{
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME )
{
m_data - > m_physicsDeltaTime = clientCmd . m_physSimParamArgs . m_deltaTime ;
}
2016-07-18 06:50:11 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION )
{
m_data - > m_allowRealTimeSimulation = clientCmd . m_physSimParamArgs . m_allowRealTimeSimulation ;
}
2016-10-23 14:14:50 +00:00
//see
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS )
{
//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
gCreateDefaultRobotAssets = ( clientCmd . m_physSimParamArgs . m_internalSimFlags & SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS ) ;
gInternalSimFlags = clientCmd . m_physSimParamArgs . m_internalSimFlags ;
}
2015-11-23 04:50:32 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_GRAVITY )
{
btVector3 grav ( clientCmd . m_physSimParamArgs . m_gravityAcceleration [ 0 ] ,
clientCmd . m_physSimParamArgs . m_gravityAcceleration [ 1 ] ,
clientCmd . m_physSimParamArgs . m_gravityAcceleration [ 2 ] ) ;
this - > m_data - > m_dynamicsWorld - > setGravity ( grav ) ;
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Updated Gravity: %f,%f,%f " , grav [ 0 ] , grav [ 1 ] , grav [ 2 ] ) ;
}
}
2016-10-05 23:31:48 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_numIterations = clientCmd . m_physSimParamArgs . m_numSolverIterations ;
}
2016-08-24 21:25:06 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS )
{
m_data - > m_numSimulationSubSteps = clientCmd . m_physSimParamArgs . m_numSimulationSubSteps ;
}
2016-06-24 14:31:17 +00:00
2016-09-08 22:15:58 +00:00
if ( clientCmd . m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP )
{
m_data - > m_dynamicsWorld - > getSolverInfo ( ) . m_erp2 = clientCmd . m_physSimParamArgs . m_defaultContactERP ;
}
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
} ;
case CMD_INIT_POSE :
{
if ( m_data - > m_verboseOutput )
{
b3Printf ( " Server Init Pose not implemented yet " ) ;
}
int bodyUniqueId = clientCmd . m_initPoseArgs . m_bodyUniqueId ;
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION )
{
btVector3 zero ( 0 , 0 , 0 ) ;
2016-06-24 14:31:17 +00:00
btAssert ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 0 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 1 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 2 ] ) ;
2015-11-23 04:50:32 +00:00
mb - > setBaseVel ( zero ) ;
mb - > setBasePos ( btVector3 (
clientCmd . m_initPoseArgs . m_initialStateQ [ 0 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 1 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 2 ] ) ) ;
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION )
{
2016-06-24 14:31:17 +00:00
btAssert ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 3 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 4 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 5 ] & &
clientCmd . m_initPoseArgs . m_hasInitialStateQ [ 6 ] ) ;
2015-11-23 04:50:32 +00:00
mb - > setBaseOmega ( btVector3 ( 0 , 0 , 0 ) ) ;
2016-08-02 16:30:33 +00:00
btQuaternion invOrn ( clientCmd . m_initPoseArgs . m_initialStateQ [ 3 ] ,
2015-11-23 04:50:32 +00:00
clientCmd . m_initPoseArgs . m_initialStateQ [ 4 ] ,
clientCmd . m_initPoseArgs . m_initialStateQ [ 5 ] ,
2016-08-02 16:30:33 +00:00
clientCmd . m_initPoseArgs . m_initialStateQ [ 6 ] ) ;
2016-09-01 17:45:14 +00:00
2016-08-02 16:30:33 +00:00
mb - > setWorldToBaseRot ( invOrn . inverse ( ) ) ;
2015-11-23 04:50:32 +00:00
}
if ( clientCmd . m_updateFlags & INIT_POSE_HAS_JOINT_STATE )
{
int dofIndex = 7 ;
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
2016-06-24 14:31:17 +00:00
if ( ( clientCmd . m_initPoseArgs . m_hasInitialStateQ [ dofIndex ] ) & & ( mb - > getLink ( i ) . m_dofCount = = 1 ) )
2015-11-23 04:50:32 +00:00
{
mb - > setJointPos ( i , clientCmd . m_initPoseArgs . m_initialStateQ [ dofIndex ] ) ;
mb - > setJointVel ( i , 0 ) ;
}
dofIndex + = mb - > getLink ( i ) . m_dofCount ;
}
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
case CMD_RESET_SIMULATION :
{
//clean up all data
2016-08-10 01:40:12 +00:00
deleteCachedInverseDynamicsBodies ( ) ;
2016-07-08 02:24:44 +00:00
if ( m_data & & m_data - > m_guiHelper )
{
m_data - > m_guiHelper - > removeAllGraphicsInstances ( ) ;
}
2016-06-01 05:55:13 +00:00
if ( m_data )
{
m_data - > m_visualConverter . resetAll ( ) ;
}
2016-09-01 17:45:14 +00:00
2015-11-23 04:50:32 +00:00
deleteDynamicsWorld ( ) ;
createEmptyDynamicsWorld ( ) ;
2016-09-01 17:45:14 +00:00
2015-11-23 04:50:32 +00:00
m_data - > exitHandles ( ) ;
m_data - > initHandles ( ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RESET_SIMULATION_COMPLETED ;
hasStatus = true ;
2016-07-18 06:50:11 +00:00
m_data - > m_hasGround = false ;
2016-09-08 22:15:58 +00:00
m_data - > m_gripperRigidbodyFixed = 0 ;
2015-11-23 04:50:32 +00:00
break ;
}
case CMD_CREATE_RIGID_BODY :
case CMD_CREATE_BOX_COLLISION_SHAPE :
{
btVector3 halfExtents ( 1 , 1 , 1 ) ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS )
{
halfExtents = btVector3 (
clientCmd . m_createBoxShapeArguments . m_halfExtentsX ,
clientCmd . m_createBoxShapeArguments . m_halfExtentsY ,
clientCmd . m_createBoxShapeArguments . m_halfExtentsZ ) ;
}
btTransform startTrans ;
startTrans . setIdentity ( ) ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION )
{
startTrans . setOrigin ( btVector3 (
clientCmd . m_createBoxShapeArguments . m_initialPosition [ 0 ] ,
clientCmd . m_createBoxShapeArguments . m_initialPosition [ 1 ] ,
clientCmd . m_createBoxShapeArguments . m_initialPosition [ 2 ] ) ) ;
}
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
startTrans . setRotation ( btQuaternion (
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 0 ] ,
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 1 ] ,
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 2 ] ,
clientCmd . m_createBoxShapeArguments . m_initialOrientation [ 3 ] ) ) ;
}
btScalar mass = 0.f ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_MASS )
{
mass = clientCmd . m_createBoxShapeArguments . m_mass ;
}
int shapeType = COLLISION_SHAPE_TYPE_BOX ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE )
{
shapeType = clientCmd . m_createBoxShapeArguments . m_collisionShapeType ;
}
btBulletWorldImporter * worldImporter = new btBulletWorldImporter ( m_data - > m_dynamicsWorld ) ;
m_data - > m_worldImporters . push_back ( worldImporter ) ;
btCollisionShape * shape = 0 ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
switch ( shapeType )
{
case COLLISION_SHAPE_TYPE_CYLINDER_X :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 0 ] ;
shape = worldImporter - > createCylinderShapeX ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CYLINDER_Y :
{
btScalar radius = halfExtents [ 0 ] ;
btScalar height = halfExtents [ 1 ] ;
shape = worldImporter - > createCylinderShapeY ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CYLINDER_Z :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 2 ] ;
shape = worldImporter - > createCylinderShapeZ ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CAPSULE_X :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 0 ] ;
shape = worldImporter - > createCapsuleShapeX ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CAPSULE_Y :
{
btScalar radius = halfExtents [ 0 ] ;
btScalar height = halfExtents [ 1 ] ;
shape = worldImporter - > createCapsuleShapeY ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_CAPSULE_Z :
{
btScalar radius = halfExtents [ 1 ] ;
btScalar height = halfExtents [ 2 ] ;
shape = worldImporter - > createCapsuleShapeZ ( radius , height ) ;
break ;
}
case COLLISION_SHAPE_TYPE_SPHERE :
{
btScalar radius = halfExtents [ 0 ] ;
shape = worldImporter - > createSphereShape ( radius ) ;
break ;
}
case COLLISION_SHAPE_TYPE_BOX :
default :
{
shape = worldImporter - > createBoxShape ( halfExtents ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
bool isDynamic = ( mass > 0 ) ;
btRigidBody * rb = worldImporter - > createRigidBody ( isDynamic , mass , startTrans , shape , 0 ) ;
//m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
btVector4 colorRGBA ( 1 , 0 , 0 , 1 ) ;
if ( clientCmd . m_updateFlags & BOX_SHAPE_HAS_COLOR )
{
colorRGBA [ 0 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 0 ] ;
colorRGBA [ 1 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 1 ] ;
colorRGBA [ 2 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 2 ] ;
colorRGBA [ 3 ] = clientCmd . m_createBoxShapeArguments . m_colorRGBA [ 3 ] ;
}
m_data - > m_guiHelper - > createCollisionShapeGraphicsObject ( rb - > getCollisionShape ( ) ) ;
m_data - > m_guiHelper - > createCollisionObjectGraphicsObject ( rb , colorRGBA ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_RIGID_BODY_CREATION_COMPLETED ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
int bodyUniqueId = m_data - > allocHandle ( ) ;
InternalBodyHandle * bodyHandle = m_data - > getHandle ( bodyUniqueId ) ;
serverCmd . m_rigidBodyCreateArgs . m_bodyUniqueId = bodyUniqueId ;
2016-09-01 20:30:07 +00:00
rb - > setUserIndex2 ( bodyUniqueId ) ;
2015-11-23 04:50:32 +00:00
bodyHandle - > m_rootLocalInertialFrame . setIdentity ( ) ;
bodyHandle - > m_rigidBody = rb ;
hasStatus = true ;
break ;
}
case CMD_PICK_BODY :
{
pickBody ( btVector3 ( clientCmd . m_pickBodyArguments . m_rayFromWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 2 ] ) ,
btVector3 ( clientCmd . m_pickBodyArguments . m_rayToWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 2 ] ) ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
case CMD_MOVE_PICKED_BODY :
{
movePickedBody ( btVector3 ( clientCmd . m_pickBodyArguments . m_rayFromWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayFromWorld [ 2 ] ) ,
btVector3 ( clientCmd . m_pickBodyArguments . m_rayToWorld [ 0 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 1 ] ,
clientCmd . m_pickBodyArguments . m_rayToWorld [ 2 ] ) ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
case CMD_REMOVE_PICKING_CONSTRAINT_BODY :
{
removePickingConstraint ( ) ;
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
2016-09-01 20:30:07 +00:00
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_sendContactPointArgs . m_numContactPointsCopied = 0 ;
//make a snapshot of the contact manifolds into individual contact points
if ( clientCmd . m_requestContactPointArguments . m_startingContactPointIndex = = 0 )
{
int numContactManifolds = m_data - > m_dynamicsWorld - > getDispatcher ( ) - > getNumManifolds ( ) ;
m_data - > m_cachedContactPoints . resize ( 0 ) ;
m_data - > m_cachedContactPoints . reserve ( numContactManifolds * 4 ) ;
for ( int i = 0 ; i < numContactManifolds ; i + + )
{
const btPersistentManifold * manifold = m_data - > m_dynamicsWorld - > getDispatcher ( ) - > getInternalManifoldPointer ( ) [ i ] ;
2016-09-02 01:28:39 +00:00
int linkIndexA = - 1 ;
int linkIndexB = - 1 ;
2016-09-01 20:30:07 +00:00
int objectIndexB = - 1 ;
const btRigidBody * bodyB = btRigidBody : : upcast ( manifold - > getBody1 ( ) ) ;
if ( bodyB )
{
objectIndexB = bodyB - > getUserIndex2 ( ) ;
}
const btMultiBodyLinkCollider * mblB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
if ( mblB & & mblB - > m_multiBody )
{
2016-09-02 01:28:39 +00:00
linkIndexB = mblB - > m_link ;
2016-09-01 20:30:07 +00:00
objectIndexB = mblB - > m_multiBody - > getUserIndex2 ( ) ;
}
int objectIndexA = - 1 ;
const btRigidBody * bodyA = btRigidBody : : upcast ( manifold - > getBody0 ( ) ) ;
if ( bodyA )
{
objectIndexA = bodyA - > getUserIndex2 ( ) ;
}
const btMultiBodyLinkCollider * mblA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
if ( mblA & & mblA - > m_multiBody )
{
2016-09-02 01:28:39 +00:00
linkIndexA = mblA - > m_link ;
2016-09-01 20:30:07 +00:00
objectIndexA = mblA - > m_multiBody - > getUserIndex2 ( ) ;
}
btAssert ( bodyA | | mblA ) ;
2016-09-02 01:28:39 +00:00
//apply the filter, if the user provides it
2016-09-01 20:30:07 +00:00
if ( clientCmd . m_requestContactPointArguments . m_objectAIndexFilter > = 0 )
{
if ( ( clientCmd . m_requestContactPointArguments . m_objectAIndexFilter ! = objectIndexA ) & &
( clientCmd . m_requestContactPointArguments . m_objectAIndexFilter ! = objectIndexB ) )
continue ;
}
2016-09-02 01:28:39 +00:00
//apply the second object filter, if the user provides it
2016-09-01 20:30:07 +00:00
if ( clientCmd . m_requestContactPointArguments . m_objectBIndexFilter > = 0 )
{
if ( ( clientCmd . m_requestContactPointArguments . m_objectBIndexFilter ! = objectIndexA ) & &
( clientCmd . m_requestContactPointArguments . m_objectBIndexFilter ! = objectIndexB ) )
continue ;
}
for ( int p = 0 ; p < manifold - > getNumContacts ( ) ; p + + )
{
b3ContactPointData pt ;
2016-09-02 01:28:39 +00:00
pt . m_bodyUniqueIdA = objectIndexA ;
pt . m_bodyUniqueIdB = objectIndexB ;
2016-09-01 20:30:07 +00:00
const btManifoldPoint & srcPt = manifold - > getContactPoint ( p ) ;
pt . m_contactDistance = srcPt . getDistance ( ) ;
pt . m_contactFlags = 0 ;
2016-09-02 01:28:39 +00:00
pt . m_linkIndexA = linkIndexA ;
pt . m_linkIndexB = linkIndexB ;
2016-09-01 20:30:07 +00:00
for ( int j = 0 ; j < 3 ; j + + )
{
pt . m_contactNormalOnBInWS [ j ] = srcPt . m_normalWorldOnB [ j ] ;
pt . m_positionOnAInWS [ j ] = srcPt . getPositionWorldOnA ( ) [ j ] ;
pt . m_positionOnBInWS [ j ] = srcPt . getPositionWorldOnB ( ) [ j ] ;
}
2016-09-02 01:28:39 +00:00
pt . m_normalForce = srcPt . getAppliedImpulse ( ) / m_data - > m_physicsDeltaTime ;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
2016-09-01 20:30:07 +00:00
m_data - > m_cachedContactPoints . push_back ( pt ) ;
}
}
}
int numContactPoints = m_data - > m_cachedContactPoints . size ( ) ;
//b3ContactPoint
//struct b3ContactPointDynamics
int totalBytesPerContact = sizeof ( b3ContactPointData ) ;
int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1 ;
b3ContactPointData * contactData = ( b3ContactPointData * ) bufferServerToClient ;
int startContactPointIndex = clientCmd . m_requestContactPointArguments . m_startingContactPointIndex ;
int numContactPointBatch = btMin ( numContactPoints , contactPointStorage ) ;
int endContactPointIndex = startContactPointIndex + numContactPointBatch ;
for ( int i = startContactPointIndex ; i < endContactPointIndex ; i + + )
{
const b3ContactPointData & srcPt = m_data - > m_cachedContactPoints [ i ] ;
b3ContactPointData & destPt = contactData [ serverCmd . m_sendContactPointArgs . m_numContactPointsCopied ] ;
destPt = srcPt ;
serverCmd . m_sendContactPointArgs . m_numContactPointsCopied + + ;
}
serverCmd . m_sendContactPointArgs . m_startingContactPointIndex = clientCmd . m_requestContactPointArguments . m_startingContactPointIndex ;
serverCmd . m_sendContactPointArgs . m_numRemainingContactPoints = numContactPoints - clientCmd . m_requestContactPointArguments . m_startingContactPointIndex - serverCmd . m_sendContactPointArgs . m_numContactPointsCopied ;
serverCmd . m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED ; //CMD_CONTACT_POINT_INFORMATION_FAILED,
hasStatus = true ;
break ;
2015-11-23 04:50:32 +00:00
}
2016-08-10 01:40:12 +00:00
case CMD_CALCULATE_INVERSE_DYNAMICS :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
InternalBodyHandle * bodyHandle = m_data - > getHandle ( clientCmd . m_calculateInverseDynamicsArguments . m_bodyUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
2016-09-13 22:37:46 +00:00
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED ;
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
2016-08-10 01:40:12 +00:00
if ( tree )
{
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
const int num_dofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
btInverseDynamics : : vecx nu ( num_dofs + baseDofs ) , qdot ( num_dofs + baseDofs ) , q ( num_dofs + baseDofs ) , joint_force ( num_dofs + baseDofs ) ;
for ( int i = 0 ; i < num_dofs ; i + + )
{
q [ i + baseDofs ] = clientCmd . m_calculateInverseDynamicsArguments . m_jointPositionsQ [ i ] ;
qdot [ i + baseDofs ] = clientCmd . m_calculateInverseDynamicsArguments . m_jointVelocitiesQdot [ i ] ;
nu [ i + baseDofs ] = clientCmd . m_calculateInverseDynamicsArguments . m_jointAccelerations [ i ] ;
}
2016-09-01 17:45:14 +00:00
// Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
2016-09-02 01:28:39 +00:00
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
2016-08-10 01:40:12 +00:00
{
serverCmd . m_inverseDynamicsResultArgs . m_bodyUniqueId = clientCmd . m_calculateInverseDynamicsArguments . m_bodyUniqueId ;
serverCmd . m_inverseDynamicsResultArgs . m_dofCount = num_dofs ;
for ( int i = 0 ; i < num_dofs ; i + + )
{
serverCmd . m_inverseDynamicsResultArgs . m_jointForces [ i ] = joint_force [ i + baseDofs ] ;
}
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED ;
}
else
{
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED ;
}
}
2016-09-01 17:45:14 +00:00
2016-08-10 01:40:12 +00:00
}
else
{
serverCmd . m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED ;
}
hasStatus = true ;
break ;
}
2016-09-07 23:00:38 +00:00
case CMD_CALCULATE_JACOBIAN :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
InternalBodyHandle * bodyHandle = m_data - > getHandle ( clientCmd . m_calculateJacobianArguments . m_bodyUniqueId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_FAILED ;
2016-09-13 22:37:46 +00:00
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
2016-09-07 23:00:38 +00:00
if ( tree )
{
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
const int num_dofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
btInverseDynamics : : vecx nu ( num_dofs + baseDofs ) , qdot ( num_dofs + baseDofs ) , q ( num_dofs + baseDofs ) , joint_force ( num_dofs + baseDofs ) ;
for ( int i = 0 ; i < num_dofs ; i + + )
{
q [ i + baseDofs ] = clientCmd . m_calculateJacobianArguments . m_jointPositionsQ [ i ] ;
qdot [ i + baseDofs ] = clientCmd . m_calculateJacobianArguments . m_jointVelocitiesQdot [ i ] ;
nu [ i + baseDofs ] = clientCmd . m_calculateJacobianArguments . m_jointAccelerations [ i ] ;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
{
serverCmd . m_jacobianResultArgs . m_dofCount = num_dofs ;
// Set jacobian value
tree - > calculateJacobians ( q ) ;
btInverseDynamics : : mat3x jac_t ( 3 , num_dofs ) ;
tree - > getBodyJacobianTrans ( clientCmd . m_calculateJacobianArguments . m_linkIndex , & jac_t ) ;
for ( int i = 0 ; i < 3 ; + + i )
{
for ( int j = 0 ; j < num_dofs ; + + j )
{
serverCmd . m_jacobianResultArgs . m_linearJacobian [ i * num_dofs + j ] = jac_t ( i , j ) ;
}
}
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_COMPLETED ;
}
else
{
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_FAILED ;
}
}
}
else
{
serverCmd . m_type = CMD_CALCULATED_JACOBIAN_FAILED ;
}
hasStatus = true ;
break ;
}
2016-06-27 01:18:30 +00:00
case CMD_APPLY_EXTERNAL_FORCE :
{
2016-07-08 02:24:44 +00:00
if ( m_data - > m_verboseOutput )
2016-09-13 22:37:46 +00:00
{
b3Printf ( " CMD_APPLY_EXTERNAL_FORCE clientCmd = %d \n " , clientCmd . m_sequenceNumber ) ;
}
2016-06-27 01:18:30 +00:00
for ( int i = 0 ; i < clientCmd . m_externalForceArguments . m_numForcesAndTorques ; + + i )
{
InteralBodyData * body = m_data - > getHandle ( clientCmd . m_externalForceArguments . m_bodyUniqueIds [ i ] ) ;
if ( body & & body - > m_multiBody )
{
btMultiBody * mb = body - > m_multiBody ;
bool isLinkFrame = ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_LINK_FRAME ) ! = 0 ) ;
2016-09-01 17:45:14 +00:00
2016-06-27 01:18:30 +00:00
if ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_FORCE ) ! = 0 )
{
btVector3 forceLocal ( clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 2 ] ) ;
btVector3 positionLocal (
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_positions [ i * 3 + 2 ] ) ;
2016-09-01 17:45:14 +00:00
2016-06-27 01:18:30 +00:00
if ( clientCmd . m_externalForceArguments . m_linkIds [ i ] = = - 1 )
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb - > getBaseWorldTransform ( ) . getBasis ( ) * forceLocal ;
btVector3 relPosWorld = isLinkFrame ? positionLocal : mb - > getBaseWorldTransform ( ) . getBasis ( ) * positionLocal ;
mb - > addBaseForce ( forceWorld ) ;
mb - > addBaseTorque ( relPosWorld . cross ( forceWorld ) ) ;
//b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
} else
{
int link = clientCmd . m_externalForceArguments . m_linkIds [ i ] ;
btVector3 forceWorld = mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * forceLocal ;
btVector3 relPosWorld = mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * positionLocal ;
mb - > addLinkForce ( link , forceWorld ) ;
mb - > addLinkTorque ( link , relPosWorld . cross ( forceWorld ) ) ;
//b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
}
}
if ( ( clientCmd . m_externalForceArguments . m_forceFlags [ i ] & EF_TORQUE ) ! = 0 )
{
btVector3 torqueLocal ( clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 0 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 1 ] ,
clientCmd . m_externalForceArguments . m_forcesAndTorques [ i * 3 + 2 ] ) ;
2016-09-01 17:45:14 +00:00
2016-06-27 01:18:30 +00:00
if ( clientCmd . m_externalForceArguments . m_linkIds [ i ] = = - 1 )
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb - > getBaseWorldTransform ( ) . getBasis ( ) * torqueLocal ;
mb - > addBaseTorque ( torqueWorld ) ;
//b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
} else
{
int link = clientCmd . m_externalForceArguments . m_linkIds [ i ] ;
btVector3 torqueWorld = mb - > getLink ( link ) . m_cachedWorldTransform . getBasis ( ) * torqueLocal ;
mb - > addLinkTorque ( link , torqueWorld ) ;
//b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}
}
}
}
2016-09-01 17:45:14 +00:00
2016-06-27 01:18:30 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
hasStatus = true ;
break ;
}
2016-08-23 01:14:29 +00:00
case CMD_CREATE_JOINT :
{
InteralBodyData * parentBody = m_data - > getHandle ( clientCmd . m_createJointArguments . m_parentBodyIndex ) ;
if ( parentBody & & parentBody - > m_multiBody )
{
InteralBodyData * childBody = m_data - > getHandle ( clientCmd . m_createJointArguments . m_childBodyIndex ) ;
if ( childBody )
{
btVector3 pivotInParent ( clientCmd . m_createJointArguments . m_parentFrame [ 0 ] , clientCmd . m_createJointArguments . m_parentFrame [ 1 ] , clientCmd . m_createJointArguments . m_parentFrame [ 2 ] ) ;
btVector3 pivotInChild ( clientCmd . m_createJointArguments . m_childFrame [ 0 ] , clientCmd . m_createJointArguments . m_childFrame [ 1 ] , clientCmd . m_createJointArguments . m_childFrame [ 2 ] ) ;
btMatrix3x3 frameInParent ( btQuaternion ( clientCmd . m_createJointArguments . m_parentFrame [ 3 ] , clientCmd . m_createJointArguments . m_parentFrame [ 4 ] , clientCmd . m_createJointArguments . m_parentFrame [ 5 ] , clientCmd . m_createJointArguments . m_parentFrame [ 6 ] ) ) ;
btMatrix3x3 frameInChild ( btQuaternion ( clientCmd . m_createJointArguments . m_childFrame [ 3 ] , clientCmd . m_createJointArguments . m_childFrame [ 4 ] , clientCmd . m_createJointArguments . m_childFrame [ 5 ] , clientCmd . m_createJointArguments . m_childFrame [ 6 ] ) ) ;
2016-08-26 17:35:10 +00:00
btVector3 jointAxis ( clientCmd . m_createJointArguments . m_jointAxis [ 0 ] , clientCmd . m_createJointArguments . m_jointAxis [ 1 ] , clientCmd . m_createJointArguments . m_jointAxis [ 2 ] ) ;
2016-09-21 19:08:03 +00:00
if ( clientCmd . m_createJointArguments . m_jointType = = eFixedType )
2016-08-23 01:14:29 +00:00
{
2016-08-26 17:35:10 +00:00
if ( childBody - > m_multiBody )
{
btMultiBodyFixedConstraint * multibodyFixed = new btMultiBodyFixedConstraint ( parentBody - > m_multiBody , clientCmd . m_createJointArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_createJointArguments . m_childJointIndex , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
2016-09-21 19:08:03 +00:00
multibodyFixed - > setMaxAppliedImpulse ( 500.0 ) ;
2016-08-26 17:35:10 +00:00
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( multibodyFixed ) ;
}
else
{
btMultiBodyFixedConstraint * rigidbodyFixed = new btMultiBodyFixedConstraint ( parentBody - > m_multiBody , clientCmd . m_createJointArguments . m_parentJointIndex , childBody - > m_rigidBody , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
2016-09-21 19:08:03 +00:00
rigidbodyFixed - > setMaxAppliedImpulse ( 500.0 ) ;
2016-08-26 17:35:10 +00:00
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( rigidbodyFixed ) ;
}
2016-08-23 01:14:29 +00:00
}
2016-09-21 19:08:03 +00:00
else if ( clientCmd . m_createJointArguments . m_jointType = = ePrismaticType )
2016-08-23 01:14:29 +00:00
{
2016-08-26 17:35:10 +00:00
if ( childBody - > m_multiBody )
{
btMultiBodySliderConstraint * multibodySlider = new btMultiBodySliderConstraint ( parentBody - > m_multiBody , clientCmd . m_createJointArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_createJointArguments . m_childJointIndex , pivotInParent , pivotInChild , frameInParent , frameInChild , jointAxis ) ;
2016-09-21 19:08:03 +00:00
multibodySlider - > setMaxAppliedImpulse ( 500.0 ) ;
2016-08-26 17:35:10 +00:00
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( multibodySlider ) ;
}
else
{
btMultiBodySliderConstraint * rigidbodySlider = new btMultiBodySliderConstraint ( parentBody - > m_multiBody , clientCmd . m_createJointArguments . m_parentJointIndex , childBody - > m_rigidBody , pivotInParent , pivotInChild , frameInParent , frameInChild , jointAxis ) ;
2016-09-21 19:08:03 +00:00
rigidbodySlider - > setMaxAppliedImpulse ( 500.0 ) ;
2016-08-26 17:35:10 +00:00
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( rigidbodySlider ) ;
}
2016-09-28 18:17:11 +00:00
} else if ( clientCmd . m_createJointArguments . m_jointType = = ePoint2PointType )
{
if ( childBody - > m_multiBody )
{
btMultiBodyPoint2Point * p2p = new btMultiBodyPoint2Point ( parentBody - > m_multiBody , clientCmd . m_createJointArguments . m_parentJointIndex , childBody - > m_multiBody , clientCmd . m_createJointArguments . m_childJointIndex , pivotInParent , pivotInChild ) ;
p2p - > setMaxAppliedImpulse ( 500 ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( p2p ) ;
}
else
{
btMultiBodyPoint2Point * p2p = new btMultiBodyPoint2Point ( parentBody - > m_multiBody , clientCmd . m_createJointArguments . m_parentJointIndex , childBody - > m_rigidBody , pivotInParent , pivotInChild ) ;
p2p - > setMaxAppliedImpulse ( 500 ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( p2p ) ;
}
2016-08-23 01:14:29 +00:00
}
2016-09-28 18:17:11 +00:00
2016-08-23 01:14:29 +00:00
}
}
2016-09-08 22:15:58 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CLIENT_COMMAND_COMPLETED ;
2016-08-23 01:14:29 +00:00
hasStatus = true ;
break ;
}
2016-09-08 22:15:58 +00:00
case CMD_CALCULATE_INVERSE_KINEMATICS :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED ;
2016-09-22 20:27:09 +00:00
InternalBodyHandle * bodyHandle = m_data - > getHandle ( clientCmd . m_calculateInverseKinematicsArguments . m_bodyUniqueId ) ;
2016-09-08 22:15:58 +00:00
if ( bodyHandle & & bodyHandle - > m_multiBody )
{
IKTrajectoryHelper * * ikHelperPtrPtr = m_data - > m_inverseKinematicsHelpers . find ( bodyHandle - > m_multiBody ) ;
IKTrajectoryHelper * ikHelperPtr = 0 ;
if ( ikHelperPtrPtr )
{
ikHelperPtr = * ikHelperPtrPtr ;
}
else
{
IKTrajectoryHelper * tmpHelper = new IKTrajectoryHelper ;
2016-09-22 20:27:09 +00:00
m_data - > m_inverseKinematicsHelpers . insert ( bodyHandle - > m_multiBody , tmpHelper ) ;
ikHelperPtr = tmpHelper ;
2016-09-08 22:15:58 +00:00
}
2016-09-22 20:27:09 +00:00
int endEffectorLinkIndex = clientCmd . m_calculateInverseKinematicsArguments . m_endEffectorLinkIndex ;
2016-09-13 22:37:46 +00:00
2016-09-22 20:27:09 +00:00
if ( ikHelperPtr & & ( endEffectorLinkIndex < bodyHandle - > m_multiBody - > getNumLinks ( ) ) )
2016-09-08 22:15:58 +00:00
{
2016-09-22 20:27:09 +00:00
const int numDofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
2016-09-13 22:37:46 +00:00
b3AlignedObjectArray < double > jacobian_linear ;
2016-09-22 20:27:09 +00:00
jacobian_linear . resize ( 3 * numDofs ) ;
2016-09-20 00:04:05 +00:00
b3AlignedObjectArray < double > jacobian_angular ;
2016-09-22 20:27:09 +00:00
jacobian_angular . resize ( 3 * numDofs ) ;
2016-09-13 22:37:46 +00:00
int jacSize = 0 ;
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
2016-09-22 20:27:09 +00:00
btAlignedObjectArray < double > q_current ;
q_current . resize ( numDofs ) ;
2016-09-13 22:37:46 +00:00
if ( tree )
{
jacSize = jacobian_linear . size ( ) ;
// Set jacobian value
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
2016-09-22 20:27:09 +00:00
btInverseDynamics : : vecx nu ( numDofs + baseDofs ) , qdot ( numDofs + baseDofs ) , q ( numDofs + baseDofs ) , joint_force ( numDofs + baseDofs ) ;
for ( int i = 0 ; i < numDofs ; i + + )
2016-09-13 22:37:46 +00:00
{
q_current [ i ] = bodyHandle - > m_multiBody - > getJointPos ( i ) ;
q [ i + baseDofs ] = bodyHandle - > m_multiBody - > getJointPos ( i ) ;
qdot [ i + baseDofs ] = 0 ;
nu [ i + baseDofs ] = 0 ;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
{
tree - > calculateJacobians ( q ) ;
2016-09-22 20:27:09 +00:00
btInverseDynamics : : mat3x jac_t ( 3 , numDofs ) ;
btInverseDynamics : : mat3x jac_r ( 3 , numDofs ) ;
2016-09-13 22:37:46 +00:00
tree - > getBodyJacobianTrans ( endEffectorLinkIndex , & jac_t ) ;
2016-09-20 00:04:05 +00:00
tree - > getBodyJacobianRot ( endEffectorLinkIndex , & jac_r ) ;
2016-09-13 22:37:46 +00:00
for ( int i = 0 ; i < 3 ; + + i )
{
2016-09-22 20:27:09 +00:00
for ( int j = 0 ; j < numDofs ; + + j )
2016-09-13 22:37:46 +00:00
{
2016-09-22 20:27:09 +00:00
jacobian_linear [ i * numDofs + j ] = jac_t ( i , j ) ;
jacobian_angular [ i * numDofs + j ] = jac_r ( i , j ) ;
2016-09-13 22:37:46 +00:00
}
}
}
}
2016-09-22 20:27:09 +00:00
btAlignedObjectArray < double > q_new ;
q_new . resize ( numDofs ) ;
2016-09-30 07:05:00 +00:00
int ikMethod = 0 ;
2016-09-30 07:15:51 +00:00
if ( ( clientCmd . m_updateFlags & IK_HAS_TARGET_ORIENTATION ) & & ( clientCmd . m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY ) )
2016-09-30 07:05:00 +00:00
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE ;
}
else if ( clientCmd . m_updateFlags & IK_HAS_TARGET_ORIENTATION )
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION ;
}
else if ( clientCmd . m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY )
{
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE ;
}
else
{
ikMethod = IK2_VEL_DLS ;
}
2016-09-30 07:15:51 +00:00
if ( clientCmd . m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY )
2016-09-30 07:05:00 +00:00
{
btAlignedObjectArray < double > lower_limit ;
btAlignedObjectArray < double > upper_limit ;
btAlignedObjectArray < double > joint_range ;
2016-09-30 08:03:40 +00:00
btAlignedObjectArray < double > rest_pose ;
2016-09-30 07:05:00 +00:00
lower_limit . resize ( numDofs ) ;
upper_limit . resize ( numDofs ) ;
joint_range . resize ( numDofs ) ;
2016-09-30 08:03:40 +00:00
rest_pose . resize ( numDofs ) ;
2016-09-30 07:05:00 +00:00
for ( int i = 0 ; i < numDofs ; + + i )
{
lower_limit [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_lowerLimit [ i ] ;
upper_limit [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_upperLimit [ i ] ;
joint_range [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_jointRange [ i ] ;
2016-09-30 08:03:40 +00:00
rest_pose [ i ] = clientCmd . m_calculateInverseKinematicsArguments . m_restPose [ i ] ;
2016-09-30 07:05:00 +00:00
}
2016-09-30 23:47:33 +00:00
ikHelperPtr - > computeNullspaceVel ( numDofs , & q_current [ 0 ] , & lower_limit [ 0 ] , & upper_limit [ 0 ] , & joint_range [ 0 ] , & rest_pose [ 0 ] ) ;
2016-09-30 07:05:00 +00:00
}
2016-10-12 16:57:14 +00:00
btTransform endEffectorTransformWorld = bodyHandle - > m_multiBody - > getLink ( endEffectorLinkIndex ) . m_cachedWorldTransform * bodyHandle - > m_linkLocalInertialFrames [ endEffectorLinkIndex ] . inverse ( ) ;
2016-09-13 22:37:46 +00:00
btVector3DoubleData endEffectorWorldPosition ;
2016-09-20 00:04:05 +00:00
btVector3DoubleData endEffectorWorldOrientation ;
2016-09-13 22:37:46 +00:00
2016-10-12 16:57:14 +00:00
btVector3 endEffectorPosWorld = endEffectorTransformWorld . getOrigin ( ) ;
btQuaternion endEffectorOriWorld = endEffectorTransformWorld . getRotation ( ) ;
2016-09-20 00:04:05 +00:00
btVector4 endEffectorOri ( endEffectorOriWorld . x ( ) , endEffectorOriWorld . y ( ) , endEffectorOriWorld . z ( ) , endEffectorOriWorld . w ( ) ) ;
2016-09-13 22:37:46 +00:00
endEffectorPosWorld . serializeDouble ( endEffectorWorldPosition ) ;
2016-09-20 00:04:05 +00:00
endEffectorOri . serializeDouble ( endEffectorWorldOrientation ) ;
2016-09-27 23:37:49 +00:00
double dampIK [ 6 ] = { 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 1.0 } ;
2016-09-20 00:04:05 +00:00
ikHelperPtr - > computeIK ( clientCmd . m_calculateInverseKinematicsArguments . m_targetPosition , clientCmd . m_calculateInverseKinematicsArguments . m_targetOrientation ,
endEffectorWorldPosition . m_floats , endEffectorWorldOrientation . m_floats ,
2016-09-22 20:27:09 +00:00
& q_current [ 0 ] ,
numDofs , clientCmd . m_calculateInverseKinematicsArguments . m_endEffectorLinkIndex ,
2016-09-27 23:37:49 +00:00
& q_new [ 0 ] , ikMethod , & jacobian_linear [ 0 ] , & jacobian_angular [ 0 ] , jacSize * 2 , dampIK ) ;
2016-09-13 22:37:46 +00:00
serverCmd . m_inverseKinematicsResultArgs . m_bodyUniqueId = clientCmd . m_calculateInverseDynamicsArguments . m_bodyUniqueId ;
2016-09-22 20:27:09 +00:00
for ( int i = 0 ; i < numDofs ; i + + )
2016-09-13 22:37:46 +00:00
{
serverCmd . m_inverseKinematicsResultArgs . m_jointPositions [ i ] = q_new [ i ] ;
}
2016-09-22 20:27:09 +00:00
serverCmd . m_inverseKinematicsResultArgs . m_dofCount = numDofs ;
2016-09-13 22:37:46 +00:00
serverCmd . m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED ;
2016-09-08 22:15:58 +00:00
}
}
hasStatus = true ;
break ;
}
2016-10-19 05:05:28 +00:00
case CMD_REQUEST_VISUAL_SHAPE_INFO :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_VISUAL_SHAPE_INFO_FAILED ;
//retrieve the visual shape information for a specific body
int totalNumVisualShapes = m_data - > m_visualConverter . getNumVisualShapes ( clientCmd . m_requestVisualShapeDataArguments . m_bodyUniqueId ) ;
int totalBytesPerVisualShape = sizeof ( b3VisualShapeData ) ;
int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1 ;
b3VisualShapeData * visualShapeStoragePtr = ( b3VisualShapeData * ) bufferServerToClient ;
int remain = totalNumVisualShapes - clientCmd . m_requestVisualShapeDataArguments . m_startingVisualShapeIndex ;
int shapeIndex = clientCmd . m_requestVisualShapeDataArguments . m_startingVisualShapeIndex ;
m_data - > m_visualConverter . getVisualShapesData ( clientCmd . m_requestVisualShapeDataArguments . m_bodyUniqueId ,
shapeIndex ,
visualShapeStoragePtr ) ;
//m_visualConverter
serverCmd . m_sendVisualShapeArgs . m_numRemainingVisualShapes = remain - 1 ;
serverCmd . m_sendVisualShapeArgs . m_numVisualShapesCopied = 1 ;
serverCmd . m_sendVisualShapeArgs . m_startingVisualShapeIndex = clientCmd . m_requestVisualShapeDataArguments . m_startingVisualShapeIndex ;
serverCmd . m_sendVisualShapeArgs . m_bodyUniqueId = clientCmd . m_requestVisualShapeDataArguments . m_bodyUniqueId ;
serverCmd . m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED ;
hasStatus = true ;
break ;
}
2016-10-20 17:56:44 +00:00
case CMD_UPDATE_VISUAL_SHAPE :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED ;
2016-10-21 18:55:27 +00:00
m_data - > m_visualConverter . activateShapeTexture ( clientCmd . m_updateVisualShapeDataArguments . m_bodyUniqueId , clientCmd . m_updateVisualShapeDataArguments . m_jointIndex , clientCmd . m_updateVisualShapeDataArguments . m_shapeIndex , clientCmd . m_updateVisualShapeDataArguments . m_textureUniqueId ) ;
2016-10-20 17:56:44 +00:00
serverCmd . m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED ;
hasStatus = true ;
break ;
}
2016-10-21 06:40:30 +00:00
case CMD_LOAD_TEXTURE :
{
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_LOAD_TEXTURE_FAILED ;
m_data - > m_visualConverter . loadTextureFile ( clientCmd . m_loadTextureArguments . m_textureFileName ) ;
serverCmd . m_type = CMD_LOAD_TEXTURE_COMPLETED ;
hasStatus = true ;
break ;
}
2015-11-23 04:50:32 +00:00
default :
{
b3Error ( " Unknown command encountered " ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
SharedMemoryStatus & serverCmd = serverStatusOut ;
serverCmd . m_type = CMD_UNKNOWN_COMMAND_FLUSHED ;
hasStatus = true ;
}
} ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
}
}
return hasStatus ;
}
2016-09-08 22:15:58 +00:00
static int skip = 1 ;
2015-11-23 04:50:32 +00:00
void PhysicsServerCommandProcessor : : renderScene ( )
{
if ( m_data - > m_guiHelper )
{
m_data - > m_guiHelper - > syncPhysicsToGraphics ( m_data - > m_dynamicsWorld ) ;
m_data - > m_guiHelper - > render ( m_data - > m_dynamicsWorld ) ;
}
2016-10-19 00:38:43 +00:00
# ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2016-10-12 18:51:04 +00:00
for ( int i = 0 ; i < m_data - > m_dynamicsWorld - > getSoftBodyArray ( ) . size ( ) ; i + + )
{
btSoftBody * psb = ( btSoftBody * ) m_data - > m_dynamicsWorld - > getSoftBodyArray ( ) [ i ] ;
if ( m_data - > m_dynamicsWorld - > getDebugDrawer ( ) & & ! ( m_data - > m_dynamicsWorld - > getDebugDrawer ( ) - > getDebugMode ( ) & ( btIDebugDraw : : DBG_DrawWireframe ) ) )
{
2016-10-17 20:01:04 +00:00
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
2016-10-12 18:51:04 +00:00
btSoftBodyHelpers : : Draw ( psb , m_data - > m_dynamicsWorld - > getDebugDrawer ( ) , m_data - > m_dynamicsWorld - > getDrawFlags ( ) ) ;
}
}
2016-10-19 00:38:43 +00:00
# endif
2015-11-23 04:50:32 +00:00
}
void PhysicsServerCommandProcessor : : physicsDebugDraw ( int debugDrawFlags )
{
if ( m_data - > m_dynamicsWorld )
{
if ( m_data - > m_dynamicsWorld - > getDebugDrawer ( ) )
{
m_data - > m_dynamicsWorld - > getDebugDrawer ( ) - > setDebugMode ( debugDrawFlags ) ;
m_data - > m_dynamicsWorld - > debugDrawWorld ( ) ;
}
}
}
2016-09-15 23:57:00 +00:00
2015-11-23 04:50:32 +00:00
bool PhysicsServerCommandProcessor : : pickBody ( const btVector3 & rayFromWorld , const btVector3 & rayToWorld )
{
if ( m_data - > m_dynamicsWorld = = 0 )
return false ;
btCollisionWorld : : ClosestRayResultCallback rayCallback ( rayFromWorld , rayToWorld ) ;
m_data - > m_dynamicsWorld - > rayTest ( rayFromWorld , rayToWorld , rayCallback ) ;
if ( rayCallback . hasHit ( ) )
{
btVector3 pickPos = rayCallback . m_hitPointWorld ;
2016-09-08 22:15:58 +00:00
gLastPickPos = pickPos ;
2015-11-23 04:50:32 +00:00
btRigidBody * body = ( btRigidBody * ) btRigidBody : : upcast ( rayCallback . m_collisionObject ) ;
if ( body )
{
//other exclusions?
if ( ! ( body - > isStaticObject ( ) | | body - > isKinematicObject ( ) ) )
{
m_data - > m_pickedBody = body ;
m_data - > m_pickedBody - > setActivationState ( DISABLE_DEACTIVATION ) ;
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body - > getCenterOfMassTransform ( ) . inverse ( ) * pickPos ;
btPoint2PointConstraint * p2p = new btPoint2PointConstraint ( * body , localPivot ) ;
m_data - > m_dynamicsWorld - > addConstraint ( p2p , true ) ;
m_data - > m_pickedConstraint = p2p ;
btScalar mousePickClamping = 30.f ;
p2p - > m_setting . m_impulseClamp = mousePickClamping ;
//very weak constraint for picking
p2p - > m_setting . m_tau = 0.001f ;
}
2016-09-08 22:15:58 +00:00
2015-11-23 04:50:32 +00:00
} else
{
btMultiBodyLinkCollider * multiCol = ( btMultiBodyLinkCollider * ) btMultiBodyLinkCollider : : upcast ( rayCallback . m_collisionObject ) ;
if ( multiCol & & multiCol - > m_multiBody )
{
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_prevCanSleep = multiCol - > m_multiBody - > getCanSleep ( ) ;
multiCol - > m_multiBody - > setCanSleep ( false ) ;
btVector3 pivotInA = multiCol - > m_multiBody - > worldPosToLocal ( multiCol - > m_link , pickPos ) ;
btMultiBodyPoint2Point * p2p = new btMultiBodyPoint2Point ( multiCol - > m_multiBody , multiCol - > m_link , 0 , pivotInA , pickPos ) ;
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling = 1 ;
p2p - > setMaxAppliedImpulse ( 2 * scaling ) ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( p2p ) ;
2016-06-24 14:31:17 +00:00
m_data - > m_pickingMultiBodyPoint2Point = p2p ;
2015-11-23 04:50:32 +00:00
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_data - > m_oldPickingPos = rayToWorld ;
m_data - > m_hitPos = pickPos ;
m_data - > m_oldPickingDist = ( pickPos - rayFromWorld ) . length ( ) ;
// printf("hit !\n");
//add p2p
}
return false ;
}
bool PhysicsServerCommandProcessor : : movePickedBody ( const btVector3 & rayFromWorld , const btVector3 & rayToWorld )
{
if ( m_data - > m_pickedBody & & m_data - > m_pickedConstraint )
{
btPoint2PointConstraint * pickCon = static_cast < btPoint2PointConstraint * > ( m_data - > m_pickedConstraint ) ;
if ( pickCon )
{
//keep it at the same picking distance
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btVector3 dir = rayToWorld - rayFromWorld ;
dir . normalize ( ) ;
dir * = m_data - > m_oldPickingDist ;
btVector3 newPivotB = rayFromWorld + dir ;
pickCon - > setPivotB ( newPivotB ) ;
}
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
if ( m_data - > m_pickingMultiBodyPoint2Point )
{
//keep it at the same picking distance
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
btVector3 dir = rayToWorld - rayFromWorld ;
dir . normalize ( ) ;
dir * = m_data - > m_oldPickingDist ;
btVector3 newPivotB = rayFromWorld + dir ;
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
m_data - > m_pickingMultiBodyPoint2Point - > setPivotInB ( newPivotB ) ;
}
2016-06-24 14:31:17 +00:00
2015-11-23 04:50:32 +00:00
return false ;
}
void PhysicsServerCommandProcessor : : removePickingConstraint ( )
{
if ( m_data - > m_pickedConstraint )
{
m_data - > m_dynamicsWorld - > removeConstraint ( m_data - > m_pickedConstraint ) ;
delete m_data - > m_pickedConstraint ;
m_data - > m_pickedConstraint = 0 ;
2016-09-24 18:25:05 +00:00
m_data - > m_pickedBody - > forceActivationState ( ACTIVE_TAG ) ;
2015-11-23 04:50:32 +00:00
m_data - > m_pickedBody = 0 ;
}
if ( m_data - > m_pickingMultiBodyPoint2Point )
{
m_data - > m_pickingMultiBodyPoint2Point - > getMultiBodyA ( ) - > setCanSleep ( m_data - > m_prevCanSleep ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > removeMultiBodyConstraint ( m_data - > m_pickingMultiBodyPoint2Point ) ;
delete m_data - > m_pickingMultiBodyPoint2Point ;
m_data - > m_pickingMultiBodyPoint2Point = 0 ;
}
}
void PhysicsServerCommandProcessor : : enableCommandLogging ( bool enable , const char * fileName )
{
if ( enable )
{
if ( 0 = = m_data - > m_commandLogger )
{
m_data - > m_commandLogger = new CommandLogger ( fileName ) ;
}
} else
{
if ( 0 ! = m_data - > m_commandLogger )
{
delete m_data - > m_commandLogger ;
m_data - > m_commandLogger = 0 ;
}
}
}
void PhysicsServerCommandProcessor : : replayFromLogFile ( const char * fileName )
{
CommandLogPlayback * pb = new CommandLogPlayback ( fileName ) ;
m_data - > m_logPlayback = pb ;
}
2016-09-23 02:48:26 +00:00
2016-09-20 19:37:13 +00:00
btVector3 gVRGripperPos ( 0 , 0 , 0.2 ) ;
2016-09-08 22:15:58 +00:00
btQuaternion gVRGripperOrn ( 0 , 0 , 0 , 1 ) ;
2016-09-27 21:10:20 +00:00
btVector3 gVRController2Pos ( 0 , 0 , 0.2 ) ;
2016-09-23 02:48:26 +00:00
btQuaternion gVRController2Orn ( 0 , 0 , 0 , 1 ) ;
2016-10-04 15:53:59 +00:00
btScalar gVRGripper2Analog = 0 ;
2016-09-15 23:57:00 +00:00
btScalar gVRGripperAnalog = 0 ;
2016-10-04 15:53:59 +00:00
2016-09-09 21:30:37 +00:00
bool gVRGripperClosed = false ;
2016-09-09 18:28:38 +00:00
int gDroppedSimulationSteps = 0 ;
int gNumSteps = 0 ;
double gDtInSec = 0.f ;
double gSubStep = 0.f ;
2016-07-18 06:50:11 +00:00
void PhysicsServerCommandProcessor : : stepSimulationRealTime ( double dtInSec )
{
2016-09-19 14:02:43 +00:00
if ( ( gEnableRealTimeSimVR | | m_data - > m_allowRealTimeSimulation ) & & m_data - > m_guiHelper )
2016-07-18 06:50:11 +00:00
{
2016-10-14 22:06:09 +00:00
2016-10-23 14:14:50 +00:00
///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
if ( gCreateDefaultRobotAssets )
2016-09-22 15:50:28 +00:00
{
2016-10-23 14:14:50 +00:00
createDefaultRobotAssets ( ) ;
2016-09-22 15:50:28 +00:00
}
2016-09-08 22:15:58 +00:00
2016-10-23 14:14:50 +00:00
int maxSteps = m_data - > m_numSimulationSubSteps + 3 ;
if ( m_data - > m_numSimulationSubSteps )
2016-07-18 06:50:11 +00:00
{
2016-10-23 14:14:50 +00:00
gSubStep = m_data - > m_physicsDeltaTime / m_data - > m_numSimulationSubSteps ;
}
else
{
gSubStep = m_data - > m_physicsDeltaTime ;
}
int numSteps = m_data - > m_dynamicsWorld - > stepSimulation ( dtInSec * simTimeScalingFactor , maxSteps , gSubStep ) ;
gDroppedSimulationSteps + = numSteps > maxSteps ? numSteps - maxSteps : 0 ;
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
if ( numSteps )
{
gNumSteps = numSteps ;
gDtInSec = dtInSec ;
}
}
}
2016-07-18 06:50:11 +00:00
2016-10-23 14:14:50 +00:00
void PhysicsServerCommandProcessor : : applyJointDamping ( int bodyUniqueId )
{
InteralBodyData * body = m_data - > getHandle ( bodyUniqueId ) ;
if ( body ) {
btMultiBody * mb = body - > m_multiBody ;
if ( mb ) {
for ( int l = 0 ; l < mb - > getNumLinks ( ) ; l + + ) {
for ( int d = 0 ; d < mb - > getLink ( l ) . m_dofCount ; d + + ) {
double damping_coefficient = mb - > getLink ( l ) . m_jointDamping ;
double damping = - damping_coefficient * mb - > getJointVelMultiDof ( l ) [ d ] ;
mb - > addJointTorqueMultiDof ( l , d , damping ) ;
}
}
}
}
}
2016-10-09 01:40:09 +00:00
2016-09-25 23:05:53 +00:00
2016-10-23 14:14:50 +00:00
//todo: move this to Python/scripting
void PhysicsServerCommandProcessor : : createDefaultRobotAssets ( )
{
static btAlignedObjectArray < char > gBufferServerToClient ;
gBufferServerToClient . resize ( SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE ) ;
int bodyId = 0 ;
if ( gCreateObjectSimVR > = 0 )
{
gCreateObjectSimVR = - 1 ;
btMatrix3x3 mat ( gVRGripperOrn ) ;
btScalar spawnDistance = 0.1 ;
btVector3 spawnDir = mat . getColumn ( 0 ) ;
btVector3 shiftPos = spawnDir * spawnDistance ;
btVector3 spawnPos = gVRGripperPos + shiftPos ;
loadUrdf ( " sphere_small.urdf " , spawnPos , gVRGripperOrn , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data - > m_sphereId = bodyId ;
InteralBodyData * parentBody = m_data - > getHandle ( bodyId ) ;
if ( parentBody - > m_multiBody )
{
parentBody - > m_multiBody - > setBaseVel ( spawnDir * 5 ) ;
}
}
if ( ! m_data - > m_hasGround )
{
m_data - > m_hasGround = true ;
loadUrdf ( " plane.urdf " , btVector3 ( 0 , 0 , 0 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , true , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " samurai.urdf " , btVector3 ( 0 , 0 , 0 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
2016-09-24 18:25:05 +00:00
2016-10-23 14:14:50 +00:00
if ( m_data - > m_gripperRigidbodyFixed = = 0 )
{
int bodyId = 0 ;
if ( loadUrdf ( " pr2_gripper.urdf " , btVector3 ( 0 , 0 , 0.1 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) )
{
InteralBodyData * parentBody = m_data - > getHandle ( bodyId ) ;
if ( parentBody - > m_multiBody )
2016-10-09 01:40:09 +00:00
{
2016-10-23 14:14:50 +00:00
parentBody - > m_multiBody - > setHasSelfCollision ( 0 ) ;
btVector3 pivotInParent ( 0.2 , 0 , 0 ) ;
2016-10-09 01:40:09 +00:00
btMatrix3x3 frameInParent ;
2016-10-23 14:14:50 +00:00
//frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
2016-10-09 01:40:09 +00:00
frameInParent . setIdentity ( ) ;
btVector3 pivotInChild ( 0 , 0 , 0 ) ;
btMatrix3x3 frameInChild ;
frameInChild . setIdentity ( ) ;
2016-10-23 14:14:50 +00:00
m_data - > m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint ( parentBody - > m_multiBody , - 1 , 0 , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
m_data - > m_gripperMultiBody = parentBody - > m_multiBody ;
if ( m_data - > m_gripperMultiBody - > getNumLinks ( ) > 2 )
{
m_data - > m_gripperMultiBody - > setJointPos ( 0 , 0 ) ;
m_data - > m_gripperMultiBody - > setJointPos ( 2 , 0 ) ;
}
m_data - > m_gripperRigidbodyFixed - > setMaxAppliedImpulse ( 500 ) ;
btMultiBodyDynamicsWorld * world = ( btMultiBodyDynamicsWorld * ) m_data - > m_dynamicsWorld ;
world - > addMultiBodyConstraint ( m_data - > m_gripperRigidbodyFixed ) ;
2016-10-09 01:40:09 +00:00
}
2016-10-23 14:14:50 +00:00
}
}
2016-09-25 23:05:53 +00:00
2016-10-23 14:14:50 +00:00
loadUrdf ( " kuka_iiwa/model_vr_limits.urdf " , btVector3 ( 1.4 , - 0.2 , 0.6 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
m_data - > m_KukaId = bodyId ;
loadUrdf ( " lego/lego.urdf " , btVector3 ( 1.0 , - 0.2 , .7 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " lego/lego.urdf " , btVector3 ( 1.0 , - 0.2 , .8 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " lego/lego.urdf " , btVector3 ( 1.0 , - 0.2 , .9 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " r2d2.urdf " , btVector3 ( - 2 , - 4 , 1 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
// Load one motor gripper for kuka
loadSdf ( " gripper/wsg50_one_motor_gripper_new_free_base.sdf " , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) , true ) ;
m_data - > m_gripperId = bodyId + 1 ;
InteralBodyData * kukaBody = m_data - > getHandle ( m_data - > m_KukaId ) ;
InteralBodyData * gripperBody = m_data - > getHandle ( m_data - > m_gripperId ) ;
// Reset the default gripper motor maximum torque for damping to 0
for ( int i = 0 ; i < gripperBody - > m_multiBody - > getNumLinks ( ) ; i + + )
{
if ( supportsJointMotor ( gripperBody - > m_multiBody , i ) )
{
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) gripperBody - > m_multiBody - > getLink ( i ) . m_userPtr ;
if ( motor )
2016-10-09 01:40:09 +00:00
{
2016-10-23 14:14:50 +00:00
motor - > setMaxAppliedImpulse ( 0 ) ;
2016-10-09 01:40:09 +00:00
}
2016-10-23 14:14:50 +00:00
}
}
for ( int i = 0 ; i < 6 ; i + + )
{
loadUrdf ( " jenga/jenga.urdf " , btVector3 ( 1.3 - 0.1 * i , - 0.7 , .75 ) , btQuaternion ( btVector3 ( 0 , 1 , 0 ) , SIMD_HALF_PI ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
}
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Add slider joint for fingers
btVector3 pivotInParent1 ( - 0.055 , 0 , 0.02 ) ;
btVector3 pivotInChild1 ( 0 , 0 , 0 ) ;
btMatrix3x3 frameInParent1 ( btQuaternion ( 0 , 0 , 0 , 1.0 ) ) ;
btMatrix3x3 frameInChild1 ( btQuaternion ( 0 , 0 , 0 , 1.0 ) ) ;
btVector3 jointAxis1 ( 1.0 , 0 , 0 ) ;
btVector3 pivotInParent2 ( 0.055 , 0 , 0.02 ) ;
btVector3 pivotInChild2 ( 0 , 0 , 0 ) ;
btMatrix3x3 frameInParent2 ( btQuaternion ( 0 , 0 , 0 , 1.0 ) ) ;
btMatrix3x3 frameInChild2 ( btQuaternion ( 0 , 0 , 1.0 , 0 ) ) ;
btVector3 jointAxis2 ( 1.0 , 0 , 0 ) ;
m_data - > m_kukaGripperRevolute1 = new btMultiBodyPoint2Point ( gripperBody - > m_multiBody , 2 , gripperBody - > m_multiBody , 4 , pivotInParent1 , pivotInChild1 ) ;
m_data - > m_kukaGripperRevolute1 - > setMaxAppliedImpulse ( 5.0 ) ;
m_data - > m_kukaGripperRevolute2 = new btMultiBodyPoint2Point ( gripperBody - > m_multiBody , 3 , gripperBody - > m_multiBody , 6 , pivotInParent2 , pivotInChild2 ) ;
m_data - > m_kukaGripperRevolute2 - > setMaxAppliedImpulse ( 5.0 ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( m_data - > m_kukaGripperRevolute1 ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( m_data - > m_kukaGripperRevolute2 ) ;
if ( kukaBody - > m_multiBody & & kukaBody - > m_multiBody - > getNumDofs ( ) = = 7 )
{
gripperBody - > m_multiBody - > setHasSelfCollision ( 0 ) ;
btVector3 pivotInParent ( 0 , 0 , 0.05 ) ;
btMatrix3x3 frameInParent ;
frameInParent . setIdentity ( ) ;
btVector3 pivotInChild ( 0 , 0 , 0 ) ;
btMatrix3x3 frameInChild ;
frameInChild . setIdentity ( ) ;
m_data - > m_kukaGripperFixed = new btMultiBodyFixedConstraint ( kukaBody - > m_multiBody , 6 , gripperBody - > m_multiBody , 0 , pivotInParent , pivotInChild , frameInParent , frameInChild ) ;
m_data - > m_kukaGripperMultiBody = gripperBody - > m_multiBody ;
m_data - > m_kukaGripperFixed - > setMaxAppliedImpulse ( 500 ) ;
m_data - > m_dynamicsWorld - > addMultiBodyConstraint ( m_data - > m_kukaGripperFixed ) ;
}
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
for ( int i = 0 ; i < 10 ; i + + )
{
loadUrdf ( " cube.urdf " , btVector3 ( - 4 , - 2 , 0.5 + i ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
}
loadUrdf ( " sphere2.urdf " , btVector3 ( - 5 , 0 , 1 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " sphere2.urdf " , btVector3 ( - 5 , 0 , 2 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " sphere2.urdf " , btVector3 ( - 5 , 0 , 3 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
btTransform objectLocalTr [ ] = {
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( 0.0 , 0.0 , 0.0 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( 0.0 , 0.15 , 0.64 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( 0.1 , 0.15 , 0.85 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( - 0.4 , 0.05 , 0.85 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( - 0.3 , - 0.05 , 0.7 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( 0.1 , 0.05 , 0.7 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( - 0.2 , 0.15 , 0.7 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( - 0.2 , 0.15 , 0.9 ) ) ,
btTransform ( btQuaternion ( 0 , 0 , 0 , 1 ) , btVector3 ( 0.2 , 0.05 , 0.8 ) )
} ;
btAlignedObjectArray < btTransform > objectWorldTr ;
int numOb = sizeof ( objectLocalTr ) / sizeof ( btTransform ) ;
objectWorldTr . resize ( numOb ) ;
btTransform tr ;
tr . setIdentity ( ) ;
tr . setRotation ( btQuaternion ( btVector3 ( 0 , 0 , 1 ) , SIMD_HALF_PI ) ) ;
tr . setOrigin ( btVector3 ( 1.0 , - 0.2 , 0 ) ) ;
for ( int i = 0 ; i < numOb ; i + + )
{
objectWorldTr [ i ] = tr * objectLocalTr [ i ] ;
}
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
// Table area
loadUrdf ( " table/table.urdf " , objectWorldTr [ 0 ] . getOrigin ( ) , objectWorldTr [ 0 ] . getRotation ( ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " tray.urdf " , objectWorldTr [ 1 ] . getOrigin ( ) , objectWorldTr [ 1 ] . getRotation ( ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf ( " teddy_vhacd.urdf " , objectWorldTr [ 4 ] . getOrigin ( ) , objectWorldTr [ 4 ] . getRotation ( ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " cube_small.urdf " , objectWorldTr [ 5 ] . getOrigin ( ) , objectWorldTr [ 5 ] . getRotation ( ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " sphere_small.urdf " , objectWorldTr [ 6 ] . getOrigin ( ) , objectWorldTr [ 6 ] . getRotation ( ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " duck_vhacd.urdf " , objectWorldTr [ 7 ] . getOrigin ( ) , objectWorldTr [ 7 ] . getRotation ( ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
//loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Shelf area
loadSdf ( " kiva_shelf/model.sdf " , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) , true ) ;
loadUrdf ( " teddy_vhacd.urdf " , btVector3 ( - 0.1 , 0.6 , 0.85 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " sphere_small.urdf " , btVector3 ( - 0.1 , 0.6 , 1.25 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
loadUrdf ( " cube_small.urdf " , btVector3 ( 0.3 , 0.6 , 0.85 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
2016-09-27 21:10:20 +00:00
2016-10-23 14:14:50 +00:00
// Chess area
loadUrdf ( " table_square/table_square.urdf " , btVector3 ( - 1.0 , 0 , 0.0 ) , btQuaternion ( 0 , 0 , 0 , 1 ) , true , false , & bodyId , & gBufferServerToClient [ 0 ] , gBufferServerToClient . size ( ) ) ;
//loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
2016-09-30 18:57:30 +00:00
2016-10-23 14:14:50 +00:00
//loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data - > m_huskyId = bodyId ;
2016-09-19 14:02:43 +00:00
2016-10-23 14:14:50 +00:00
m_data - > m_dynamicsWorld - > setGravity ( btVector3 ( 0 , 0 , - 10 ) ) ;
2016-09-20 19:37:13 +00:00
2016-10-23 14:14:50 +00:00
}
2016-07-18 06:50:11 +00:00
2016-10-23 14:14:50 +00:00
if ( m_data - > m_kukaGripperFixed & & m_data - > m_kukaGripperMultiBody )
{
InteralBodyData * childBody = m_data - > getHandle ( m_data - > m_gripperId ) ;
// Add gripper controller
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) childBody - > m_multiBody - > getLink ( 1 ) . m_userPtr ;
if ( motor )
{
btScalar posTarget = ( - 0.048 ) * btMin ( btScalar ( 0.75 ) , gVRGripper2Analog ) / 0.75 ;
motor - > setPositionTarget ( posTarget , .2 ) ;
motor - > setVelocityTarget ( 0.0 , .5 ) ;
motor - > setMaxAppliedImpulse ( 5.0 ) ;
}
}
2016-09-26 04:13:31 +00:00
2016-10-23 14:14:50 +00:00
if ( m_data - > m_gripperRigidbodyFixed & & m_data - > m_gripperMultiBody )
{
m_data - > m_gripperRigidbodyFixed - > setFrameInB ( btMatrix3x3 ( gVRGripperOrn ) ) ;
m_data - > m_gripperRigidbodyFixed - > setPivotInB ( gVRGripperPos ) ;
for ( int i = 0 ; i < m_data - > m_gripperMultiBody - > getNumLinks ( ) ; i + + )
{
if ( supportsJointMotor ( m_data - > m_gripperMultiBody , i ) )
2016-09-09 21:30:37 +00:00
{
2016-10-23 14:14:50 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) m_data - > m_gripperMultiBody - > getLink ( i ) . m_userPtr ;
if ( motor )
2016-09-09 21:30:37 +00:00
{
2016-10-23 14:14:50 +00:00
motor - > setErp ( 0.2 ) ;
btScalar posTarget = 0.1 + ( 1 - btMin ( btScalar ( 0.75 ) , gVRGripperAnalog ) * btScalar ( 1.5 ) ) * SIMD_HALF_PI * 0.29 ;
btScalar maxPosTarget = 0.55 ;
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
if ( m_data - > m_gripperMultiBody - > getJointPos ( i ) < 0 )
{
m_data - > m_gripperMultiBody - > setJointPos ( i , 0 ) ;
}
if ( m_data - > m_gripperMultiBody - > getJointPos ( i ) > maxPosTarget )
{
m_data - > m_gripperMultiBody - > setJointPos ( i , maxPosTarget ) ;
2016-09-09 21:30:37 +00:00
}
2016-10-23 14:14:50 +00:00
motor - > setPositionTarget ( posTarget , 1 ) ;
motor - > setVelocityTarget ( 0 , 0.5 ) ;
btScalar maxImp = 1 * m_data - > m_physicsDeltaTime ;
motor - > setMaxAppliedImpulse ( maxImp ) ;
//motor->setRhsClamp(gRhsClamp);
2016-09-09 21:30:37 +00:00
}
}
2016-10-23 14:14:50 +00:00
}
}
2016-09-08 22:15:58 +00:00
2016-10-23 14:14:50 +00:00
// Inverse kinematics for KUKA
//if (0)
{
InternalBodyHandle * bodyHandle = m_data - > getHandle ( m_data - > m_KukaId ) ;
if ( bodyHandle & & bodyHandle - > m_multiBody & & bodyHandle - > m_multiBody - > getNumDofs ( ) = = 7 )
{
btMultiBody * mb = bodyHandle - > m_multiBody ;
btScalar sqLen = ( mb - > getBaseWorldTransform ( ) . getOrigin ( ) - gVRController2Pos ) . length2 ( ) ;
btScalar distanceThreshold = 1.3 ;
gCloseToKuka = ( sqLen < ( distanceThreshold * distanceThreshold ) ) ;
int numDofs = bodyHandle - > m_multiBody - > getNumDofs ( ) ;
btAlignedObjectArray < double > q_new ;
btAlignedObjectArray < double > q_current ;
q_current . resize ( numDofs ) ;
for ( int i = 0 ; i < numDofs ; i + + )
2016-09-23 02:48:26 +00:00
{
2016-10-23 14:14:50 +00:00
q_current [ i ] = bodyHandle - > m_multiBody - > getJointPos ( i ) ;
}
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
q_new . resize ( numDofs ) ;
//sensible rest-pose
q_new [ 0 ] = 0 ; // -SIMD_HALF_PI;
q_new [ 1 ] = 0 ;
q_new [ 2 ] = 0 ;
q_new [ 3 ] = SIMD_HALF_PI ;
q_new [ 4 ] = 0 ;
q_new [ 5 ] = - SIMD_HALF_PI * 0.66 ;
q_new [ 6 ] = 0 ;
if ( gCloseToKuka )
{
double dampIk [ 6 ] = { 1.0 , 1.0 , 1.0 , 1.0 , 1.0 , 0.0 } ;
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
IKTrajectoryHelper * * ikHelperPtrPtr = m_data - > m_inverseKinematicsHelpers . find ( bodyHandle - > m_multiBody ) ;
IKTrajectoryHelper * ikHelperPtr = 0 ;
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
if ( ikHelperPtrPtr )
{
ikHelperPtr = * ikHelperPtrPtr ;
}
else
{
IKTrajectoryHelper * tmpHelper = new IKTrajectoryHelper ;
m_data - > m_inverseKinematicsHelpers . insert ( bodyHandle - > m_multiBody , tmpHelper ) ;
ikHelperPtr = tmpHelper ;
}
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
int endEffectorLinkIndex = 6 ;
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
if ( ikHelperPtr & & ( endEffectorLinkIndex < bodyHandle - > m_multiBody - > getNumLinks ( ) ) )
{
b3AlignedObjectArray < double > jacobian_linear ;
jacobian_linear . resize ( 3 * numDofs ) ;
b3AlignedObjectArray < double > jacobian_angular ;
jacobian_angular . resize ( 3 * numDofs ) ;
int jacSize = 0 ;
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
btInverseDynamics : : MultiBodyTree * tree = m_data - > findOrCreateTree ( bodyHandle - > m_multiBody ) ;
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
if ( tree )
{
jacSize = jacobian_linear . size ( ) ;
// Set jacobian value
int baseDofs = bodyHandle - > m_multiBody - > hasFixedBase ( ) ? 0 : 6 ;
2016-09-27 21:10:20 +00:00
2016-10-23 14:14:50 +00:00
btInverseDynamics : : vecx nu ( numDofs + baseDofs ) , qdot ( numDofs + baseDofs ) , q ( numDofs + baseDofs ) , joint_force ( numDofs + baseDofs ) ;
for ( int i = 0 ; i < numDofs ; i + + )
{
q_current [ i ] = bodyHandle - > m_multiBody - > getJointPos ( i ) ;
q [ i + baseDofs ] = bodyHandle - > m_multiBody - > getJointPos ( i ) ;
qdot [ i + baseDofs ] = 0 ;
nu [ i + baseDofs ] = 0 ;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics : : vec3 id_grav ( m_data - > m_dynamicsWorld - > getGravity ( ) ) ;
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
if ( - 1 ! = tree - > setGravityInWorldFrame ( id_grav ) & &
- 1 ! = tree - > calculateInverseDynamics ( q , qdot , nu , & joint_force ) )
{
tree - > calculateJacobians ( q ) ;
btInverseDynamics : : mat3x jac_t ( 3 , numDofs ) ;
btInverseDynamics : : mat3x jac_r ( 3 , numDofs ) ;
tree - > getBodyJacobianTrans ( endEffectorLinkIndex , & jac_t ) ;
tree - > getBodyJacobianRot ( endEffectorLinkIndex , & jac_r ) ;
for ( int i = 0 ; i < 3 ; + + i )
{
for ( int j = 0 ; j < numDofs ; + + j )
2016-10-09 01:40:09 +00:00
{
2016-10-23 14:14:50 +00:00
jacobian_linear [ i * numDofs + j ] = jac_t ( i , j ) ;
jacobian_angular [ i * numDofs + j ] = jac_r ( i , j ) ;
2016-10-09 01:40:09 +00:00
}
}
2016-10-23 14:14:50 +00:00
}
}
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
int ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE ; //IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS;
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
btVector3DoubleData endEffectorWorldPosition ;
btVector3DoubleData endEffectorWorldOrientation ;
btVector3DoubleData targetWorldPosition ;
btVector3DoubleData targetWorldOrientation ;
2016-09-23 02:48:26 +00:00
2016-10-23 14:14:50 +00:00
btVector3 endEffectorPosWorld = bodyHandle - > m_multiBody - > getLink ( endEffectorLinkIndex ) . m_cachedWorldTransform . getOrigin ( ) ;
btQuaternion endEffectorOriWorld = bodyHandle - > m_multiBody - > getLink ( endEffectorLinkIndex ) . m_cachedWorldTransform . getRotation ( ) ;
btVector4 endEffectorOri ( endEffectorOriWorld . x ( ) , endEffectorOriWorld . y ( ) , endEffectorOriWorld . z ( ) , endEffectorOriWorld . w ( ) ) ;
2016-09-25 21:24:28 +00:00
2016-10-23 14:14:50 +00:00
// Prescribed position and orientation
static btScalar time = 0.f ;
time + = 0.01 ;
btVector3 targetPos ( 0.4 - 0.4 * b3Cos ( time ) , 0 , 0.8 + 0.4 * b3Cos ( time ) ) ;
targetPos + = mb - > getBasePos ( ) ;
btVector4 downOrn ( 0 , 1 , 0 , 0 ) ;
// Controller orientation
btVector4 controllerOrn ( gVRController2Orn . x ( ) , gVRController2Orn . y ( ) , gVRController2Orn . z ( ) , gVRController2Orn . w ( ) ) ;
2016-09-23 06:21:24 +00:00
2016-10-23 14:14:50 +00:00
// Set position and orientation
endEffectorPosWorld . serializeDouble ( endEffectorWorldPosition ) ;
endEffectorOri . serializeDouble ( endEffectorWorldOrientation ) ;
downOrn . serializeDouble ( targetWorldOrientation ) ;
//targetPos.serializeDouble(targetWorldPosition);
2016-09-28 23:07:55 +00:00
2016-10-23 14:14:50 +00:00
gVRController2Pos . serializeDouble ( targetWorldPosition ) ;
2016-09-28 23:07:55 +00:00
2016-10-23 14:14:50 +00:00
//controllerOrn.serializeDouble(targetWorldOrientation);
2016-09-25 21:24:28 +00:00
2016-10-23 14:14:50 +00:00
if ( ikMethod = = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE )
{
btAlignedObjectArray < double > lower_limit ;
btAlignedObjectArray < double > upper_limit ;
btAlignedObjectArray < double > joint_range ;
btAlignedObjectArray < double > rest_pose ;
lower_limit . resize ( numDofs ) ;
upper_limit . resize ( numDofs ) ;
joint_range . resize ( numDofs ) ;
rest_pose . resize ( numDofs ) ;
lower_limit [ 0 ] = - .967 ;
lower_limit [ 1 ] = - 2.0 ;
lower_limit [ 2 ] = - 2.96 ;
lower_limit [ 3 ] = 0.19 ;
lower_limit [ 4 ] = - 2.96 ;
lower_limit [ 5 ] = - 2.09 ;
lower_limit [ 6 ] = - 3.05 ;
upper_limit [ 0 ] = .96 ;
upper_limit [ 1 ] = 2.0 ;
upper_limit [ 2 ] = 2.96 ;
upper_limit [ 3 ] = 2.29 ;
upper_limit [ 4 ] = 2.96 ;
upper_limit [ 5 ] = 2.09 ;
upper_limit [ 6 ] = 3.05 ;
joint_range [ 0 ] = 5.8 ;
joint_range [ 1 ] = 4 ;
joint_range [ 2 ] = 5.8 ;
joint_range [ 3 ] = 4 ;
joint_range [ 4 ] = 5.8 ;
joint_range [ 5 ] = 4 ;
joint_range [ 6 ] = 6 ;
rest_pose [ 0 ] = 0 ;
rest_pose [ 1 ] = 0 ;
rest_pose [ 2 ] = 0 ;
rest_pose [ 3 ] = SIMD_HALF_PI ;
rest_pose [ 4 ] = 0 ;
rest_pose [ 5 ] = - SIMD_HALF_PI * 0.66 ;
rest_pose [ 6 ] = 0 ;
ikHelperPtr - > computeNullspaceVel ( numDofs , & q_current [ 0 ] , & lower_limit [ 0 ] , & upper_limit [ 0 ] , & joint_range [ 0 ] , & rest_pose [ 0 ] ) ;
2016-09-23 02:48:26 +00:00
}
2016-10-23 14:14:50 +00:00
ikHelperPtr - > computeIK ( targetWorldPosition . m_floats , targetWorldOrientation . m_floats ,
endEffectorWorldPosition . m_floats , endEffectorWorldOrientation . m_floats ,
& q_current [ 0 ] ,
numDofs , endEffectorLinkIndex ,
& q_new [ 0 ] , ikMethod , & jacobian_linear [ 0 ] , & jacobian_angular [ 0 ] , jacSize * 2 , dampIk ) ;
}
}
//directly set the position of the links, only for debugging IK, don't use this method!
if ( 0 )
{
for ( int i = 0 ; i < mb - > getNumLinks ( ) ; i + + )
{
btScalar desiredPosition = q_new [ i ] ;
mb - > setJointPosMultiDof ( i , & desiredPosition ) ;
}
} else
{
int numMotors = 0 ;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int velIndex = 6 ; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7 ; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for ( int link = 0 ; link < mb - > getNumLinks ( ) ; link + + )
2016-09-23 02:48:26 +00:00
{
2016-10-23 14:14:50 +00:00
if ( supportsJointMotor ( mb , link ) )
2016-09-23 02:48:26 +00:00
{
2016-10-23 14:14:50 +00:00
btMultiBodyJointMotor * motor = ( btMultiBodyJointMotor * ) mb - > getLink ( link ) . m_userPtr ;
2016-10-09 01:40:09 +00:00
2016-10-23 14:14:50 +00:00
if ( motor )
{
btScalar desiredVelocity = 0.f ;
btScalar desiredPosition = q_new [ link ] ;
motor - > setRhsClamp ( gRhsClamp ) ;
//printf("link %d: %f", link, q_new[link]);
motor - > setVelocityTarget ( desiredVelocity , 1.0 ) ;
motor - > setPositionTarget ( desiredPosition , 0.6 ) ;
btScalar maxImp = 1.0 ;
2016-09-28 18:17:11 +00:00
2016-10-23 14:14:50 +00:00
motor - > setMaxAppliedImpulse ( maxImp ) ;
numMotors + + ;
2016-09-23 02:48:26 +00:00
}
}
2016-10-23 14:14:50 +00:00
velIndex + = mb - > getLink ( link ) . m_dofCount ;
posIndex + = mb - > getLink ( link ) . m_posVarCount ;
2016-09-23 02:48:26 +00:00
}
2016-10-23 14:14:50 +00:00
}
2016-10-09 01:40:09 +00:00
}
2016-10-23 14:14:50 +00:00
}
2016-07-18 06:50:11 +00:00
}
2016-10-23 14:14:50 +00:00
}