mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
a9b1544a9f
Expose inverse dynamics to Bullet shared memory API, through b3CalculateInverseDynamicsCommandInit and b3GetStatusInverseDynamicsJointForces command/status. See PhysicsClientExeample or pybullet for usage. Add option for Windows and Linux to set python_lib_dir and python_include_dir for premake and --enable_pybullet option Expose inverse dynamics to pybullet: [force] = p.calculateInverseDynamics(objectIndex,[q],[qdot],[acc]) Thanks to Jeff Bingham for the suggestion.
2451 lines
89 KiB
C++
2451 lines
89 KiB
C++
#include "PhysicsServerCommandProcessor.h"
|
|
|
|
|
|
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
|
|
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
|
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
|
|
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
|
|
#include "TinyRendererVisualShapeConverter.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
|
|
#include "LinearMath/btHashMap.h"
|
|
#include "BulletInverseDynamics/MultiBodyTree.hpp"
|
|
|
|
#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"
|
|
|
|
struct UrdfLinkNameMapUtil
|
|
{
|
|
btMultiBody* m_mb;
|
|
btDefaultSerializer* m_memSerializer;
|
|
|
|
UrdfLinkNameMapUtil():m_mb(0),m_memSerializer(0)
|
|
{
|
|
}
|
|
virtual ~UrdfLinkNameMapUtil()
|
|
{
|
|
delete m_memSerializer;
|
|
}
|
|
};
|
|
|
|
|
|
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)
|
|
{
|
|
}
|
|
|
|
virtual void setDebugMode(int debugMode)
|
|
{
|
|
m_debugMode = debugMode;
|
|
}
|
|
|
|
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;
|
|
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
|
|
|
|
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;
|
|
void SetNextFree(int next)
|
|
{
|
|
m_nextFreeHandle = next;
|
|
}
|
|
int GetNextFree() const
|
|
{
|
|
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;
|
|
|
|
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;
|
|
|
|
int ver = btGetVersion();
|
|
if (ver>=0 && ver<999)
|
|
{
|
|
sprintf((char*)&buffer[9],"%d",ver);
|
|
}
|
|
|
|
}
|
|
|
|
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=='-');
|
|
|
|
const bool VOID_IS_8 = ((sizeof(void*)==8));
|
|
m_bitsVary = (VOID_IS_8 != m_fileIs64bit);
|
|
|
|
|
|
|
|
}
|
|
virtual ~CommandLogPlayback()
|
|
{
|
|
if (m_file)
|
|
{
|
|
fclose(m_file);
|
|
m_file=0;
|
|
}
|
|
}
|
|
bool processNextCommand(SharedMemoryCommand* cmd)
|
|
{
|
|
if (m_file)
|
|
{
|
|
size_t s = 0;
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
if (s==1)
|
|
{
|
|
s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file);
|
|
return (s==1);
|
|
}
|
|
}
|
|
return false;
|
|
|
|
}
|
|
};
|
|
|
|
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;
|
|
|
|
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);
|
|
|
|
|
|
getHandle(handle)->SetNextFree(m_firstFreeHandle);
|
|
}
|
|
|
|
|
|
return handle;
|
|
}
|
|
|
|
|
|
void freeHandle(int handle)
|
|
{
|
|
btAssert(handle >= 0);
|
|
|
|
getHandle(handle)->SetNextFree(m_firstFreeHandle);
|
|
m_firstFreeHandle = handle;
|
|
|
|
m_numUsedHandles--;
|
|
}
|
|
|
|
///end handle management
|
|
|
|
bool m_allowRealTimeSimulation;
|
|
bool m_hasGround;
|
|
|
|
CommandLogger* m_commandLogger;
|
|
CommandLogPlayback* m_logPlayback;
|
|
|
|
|
|
btScalar m_physicsDeltaTime;
|
|
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
|
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
|
|
|
|
|
|
|
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;
|
|
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
|
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
|
|
|
|
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
|
|
|
|
|
|
|
|
struct GUIHelperInterface* m_guiHelper;
|
|
int m_sharedMemoryKey;
|
|
|
|
bool m_verboseOutput;
|
|
|
|
|
|
//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;
|
|
TinyRendererVisualShapeConverter m_visualConverter;
|
|
|
|
PhysicsServerCommandProcessorInternalData()
|
|
:m_hasGround(false),
|
|
m_allowRealTimeSimulation(false),
|
|
m_commandLogger(0),
|
|
m_logPlayback(0),
|
|
m_physicsDeltaTime(1./240.),
|
|
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)
|
|
{
|
|
|
|
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
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
|
|
{
|
|
|
|
if (guiHelper)
|
|
{
|
|
|
|
guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
|
|
} else
|
|
{
|
|
if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer())
|
|
{
|
|
|
|
m_data->m_dynamicsWorld->setDebugDrawer(0);
|
|
}
|
|
}
|
|
m_data->m_guiHelper = guiHelper;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
|
|
{
|
|
m_data = new PhysicsServerCommandProcessorInternalData();
|
|
|
|
createEmptyDynamicsWorld();
|
|
|
|
}
|
|
|
|
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
|
|
{
|
|
deleteDynamicsWorld();
|
|
if (m_data->m_commandLogger)
|
|
{
|
|
delete m_data->m_commandLogger;
|
|
m_data->m_commandLogger = 0;
|
|
}
|
|
|
|
delete m_data;
|
|
}
|
|
|
|
|
|
|
|
void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
|
{
|
|
///collision configuration contains default setup for memory, collision setup
|
|
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
|
|
//m_collisionConfiguration->setConvexConvexMultipointIterations();
|
|
|
|
///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);
|
|
|
|
m_data->m_broadphase = new btDbvtBroadphase();
|
|
|
|
m_data->m_solver = new btMultiBodyConstraintSolver;
|
|
|
|
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
|
|
|
|
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
|
|
|
|
|
|
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
|
|
{
|
|
deleteCachedInverseDynamicsBodies();
|
|
|
|
|
|
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
|
|
{
|
|
delete m_data->m_multiBodyJointFeedbacks[i];
|
|
}
|
|
m_data->m_multiBodyJointFeedbacks.clear();
|
|
|
|
|
|
for (int i=0;i<m_data->m_worldImporters.size();i++)
|
|
{
|
|
delete m_data->m_worldImporters[i];
|
|
}
|
|
m_data->m_worldImporters.clear();
|
|
|
|
for (int i=0;i<m_data->m_urdfLinkNameMapper.size();i++)
|
|
{
|
|
delete m_data->m_urdfLinkNameMapper[i];
|
|
}
|
|
m_data->m_urdfLinkNameMapper.clear();
|
|
|
|
|
|
for (int i=0;i<m_data->m_strings.size();i++)
|
|
{
|
|
delete m_data->m_strings[i];
|
|
}
|
|
m_data->m_strings.clear();
|
|
|
|
btAlignedObjectArray<btTypedConstraint*> constraints;
|
|
btAlignedObjectArray<btMultiBodyConstraint*> mbconstraints;
|
|
|
|
|
|
if (m_data->m_dynamicsWorld)
|
|
{
|
|
|
|
int i;
|
|
for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
|
|
{
|
|
btTypedConstraint* constraint =m_data->m_dynamicsWorld->getConstraint(i);
|
|
constraints.push_back(constraint);
|
|
m_data->m_dynamicsWorld->removeConstraint(constraint);
|
|
}
|
|
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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
|
|
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
|
|
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;
|
|
|
|
if (supportsJointMotor(mb,mbLinkIndex))
|
|
{
|
|
float maxMotorImpulse = 0.f;
|
|
int dof = 0;
|
|
btScalar desiredVelocity = 0.f;
|
|
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
|
|
//motor->setMaxAppliedImpulse(0);
|
|
mb->getLink(mbLinkIndex).m_userPtr = motor;
|
|
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
|
|
motor->finalizeMultiDof();
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes)
|
|
{
|
|
btAssert(m_data->m_dynamicsWorld);
|
|
if (!m_data->m_dynamicsWorld)
|
|
{
|
|
b3Error("loadSdf: No valid m_dynamicsWorld");
|
|
return false;
|
|
}
|
|
|
|
m_data->m_sdfRecentLoadedBodies.clear();
|
|
|
|
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
|
|
|
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);
|
|
}
|
|
|
|
btTransform rootTrans;
|
|
rootTrans.setIdentity();
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("loaded %s OK!", fileName);
|
|
}
|
|
|
|
for (int m =0; m<u2b.getNumModels();m++)
|
|
{
|
|
|
|
u2b.activateModel(m);
|
|
btMultiBody* mb = 0;
|
|
|
|
//get a body index
|
|
int bodyUniqueId = m_data->allocHandle();
|
|
|
|
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
|
|
|
{
|
|
btScalar mass = 0;
|
|
bodyHandle->m_rootLocalInertialFrame.setIdentity();
|
|
btVector3 localInertiaDiagonal(0,0,0);
|
|
int urdfLinkIndex = u2b.getRootLinkIndex();
|
|
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
|
|
}
|
|
|
|
|
|
|
|
//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);
|
|
bool useMultiBody = true;
|
|
ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true);
|
|
|
|
|
|
|
|
mb = creation.getBulletMultiBody();
|
|
if (mb)
|
|
{
|
|
bodyHandle->m_multiBody = mb;
|
|
|
|
m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
|
|
|
|
createJointMotors(mb);
|
|
|
|
|
|
//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
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
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.");
|
|
}
|
|
|
|
}
|
|
}
|
|
return loadOk;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
|
|
|
|
|
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
|
|
|
|
|
|
if (loadOk)
|
|
{
|
|
//get a body index
|
|
int bodyUniqueId = m_data->allocHandle();
|
|
if (bodyUniqueIdPtr)
|
|
*bodyUniqueIdPtr= bodyUniqueId;
|
|
|
|
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
|
|
|
{
|
|
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);
|
|
}
|
|
|
|
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);
|
|
|
|
ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
|
|
|
|
for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
|
|
{
|
|
btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
|
|
m_data->m_collisionShapes.push_back(shape);
|
|
}
|
|
|
|
btMultiBody* mb = creation.getBulletMultiBody();
|
|
if (useMultiBody)
|
|
{
|
|
|
|
|
|
if (mb)
|
|
{
|
|
|
|
bodyHandle->m_multiBody = mb;
|
|
|
|
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);
|
|
|
|
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);
|
|
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);
|
|
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();
|
|
}
|
|
|
|
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
|
m_data->m_strings.push_back(baseName);
|
|
|
|
|
|
util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str());
|
|
mb->setBaseName(baseName->c_str());
|
|
|
|
|
|
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);
|
|
|
|
return true;
|
|
} else
|
|
{
|
|
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
|
|
return false;
|
|
}
|
|
|
|
} else
|
|
{
|
|
btAssert(0);
|
|
|
|
return true;
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes)
|
|
{
|
|
if (m_data->m_logPlayback)
|
|
{
|
|
|
|
SharedMemoryCommand clientCmd;
|
|
SharedMemoryStatus serverStatus;
|
|
|
|
bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd);
|
|
if (hasCommand)
|
|
{
|
|
processCommand(clientCmd,serverStatus,bufferServerToClient,bufferSizeInBytes);
|
|
}
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
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());
|
|
|
|
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();
|
|
|
|
}
|
|
return streamSizeInBytes;
|
|
}
|
|
|
|
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
|
|
{
|
|
|
|
bool hasStatus = false;
|
|
|
|
{
|
|
///we ignore overflow of integer for now
|
|
|
|
{
|
|
|
|
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
|
|
|
|
|
|
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
|
|
#if 1
|
|
if (m_data->m_commandLogger)
|
|
{
|
|
m_data->m_commandLogger->logCommand(clientCmd);
|
|
}
|
|
#endif
|
|
|
|
//m_data->m_testBlock1->m_numProcessedClientCommands++;
|
|
|
|
//no timestamp yet
|
|
int timeStamp = 0;
|
|
|
|
//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);
|
|
}
|
|
|
|
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);
|
|
|
|
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);
|
|
}
|
|
|
|
break;
|
|
}
|
|
#endif
|
|
|
|
case CMD_REQUEST_DEBUG_LINES:
|
|
{
|
|
int curFlags =m_data->m_remoteDebugDrawer->getDebugMode();
|
|
|
|
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;
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex);
|
|
|
|
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();
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
break;
|
|
}
|
|
|
|
case CMD_REQUEST_CAMERA_IMAGE_DATA:
|
|
{
|
|
|
|
int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
|
|
int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
|
|
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
|
|
int numPixelsCopied = 0;
|
|
|
|
|
|
|
|
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
|
|
{
|
|
//m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,0,0,0,0,0,width,height,0);
|
|
}
|
|
else
|
|
{
|
|
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);
|
|
}
|
|
m_data->m_visualConverter.getWidthAndHeight(width,height);
|
|
}
|
|
|
|
|
|
|
|
int numTotalPixels = width*height;
|
|
int numRemainingPixels = numTotalPixels - startPixelIndex;
|
|
|
|
|
|
if (numRemainingPixels>0)
|
|
{
|
|
int maxNumPixels = bufferSizeInBytes/8-1;
|
|
unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
|
|
int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels);
|
|
|
|
float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4);
|
|
|
|
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
|
|
{
|
|
m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,width,height,&numPixelsCopied);
|
|
} else
|
|
{
|
|
|
|
if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0)
|
|
{
|
|
// printf("-------------------------------\nRendering\n");
|
|
|
|
|
|
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();
|
|
}
|
|
|
|
}
|
|
|
|
m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied);
|
|
}
|
|
|
|
//each pixel takes 4 RGBA values and 1 float = 8 bytes
|
|
|
|
} else
|
|
{
|
|
|
|
}
|
|
|
|
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;
|
|
hasStatus = true;
|
|
|
|
break;
|
|
}
|
|
|
|
case CMD_REQUEST_BODY_INFO:
|
|
{
|
|
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
|
|
//stream info into memory
|
|
int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
|
|
|
|
serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
|
|
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
|
|
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes;
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
case CMD_LOAD_SDF:
|
|
{
|
|
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName);
|
|
}
|
|
|
|
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes);
|
|
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];
|
|
}
|
|
|
|
serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
|
|
} else
|
|
{
|
|
serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
|
|
}
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
case CMD_LOAD_URDF:
|
|
{
|
|
|
|
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);
|
|
|
|
|
|
if (completedOk)
|
|
{
|
|
|
|
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
|
|
|
|
serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
|
|
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0;
|
|
|
|
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;
|
|
|
|
} else
|
|
{
|
|
serverStatusOut.m_type = CMD_URDF_LOADING_FAILED;
|
|
hasStatus = true;
|
|
}
|
|
|
|
|
|
|
|
|
|
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);
|
|
};
|
|
|
|
} 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);
|
|
};
|
|
|
|
}
|
|
}
|
|
|
|
} 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);
|
|
|
|
|
|
}
|
|
*/
|
|
#endif
|
|
|
|
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
|
hasStatus = true;
|
|
|
|
break;
|
|
}
|
|
case CMD_SEND_DESIRED_STATE:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Processed CMD_SEND_DESIRED_STATE");
|
|
}
|
|
|
|
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);
|
|
|
|
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
|
|
{
|
|
case CONTROL_MODE_TORQUE:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Using CONTROL_MODE_TORQUE");
|
|
}
|
|
// mb->clearForcesAndTorques();
|
|
int torqueIndex = 0;
|
|
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
|
|
{
|
|
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++;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case CONTROL_MODE_VELOCITY:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Using CONTROL_MODE_VELOCITY");
|
|
}
|
|
|
|
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))
|
|
{
|
|
|
|
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
|
|
|
|
|
|
if (motor)
|
|
{
|
|
btScalar desiredVelocity = 0.f;
|
|
bool hasDesiredVelocity = false;
|
|
|
|
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0)
|
|
{
|
|
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
|
|
btScalar kd = 0.1f;
|
|
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
|
|
{
|
|
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
|
|
}
|
|
|
|
motor->setVelocityTarget(desiredVelocity,kd);
|
|
|
|
btScalar kp = 0.f;
|
|
motor->setPositionTarget(0,kp);
|
|
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);
|
|
}
|
|
numMotors++;
|
|
|
|
}
|
|
}
|
|
dofIndex += mb->getLink(link).m_dofCount;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
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))
|
|
{
|
|
|
|
|
|
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
|
|
|
|
if (motor)
|
|
{
|
|
|
|
bool hasDesiredPosOrVel = false;
|
|
btScalar kp = 0.f;
|
|
btScalar kd = 0.f;
|
|
btScalar desiredVelocity = 0.f;
|
|
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0)
|
|
{
|
|
hasDesiredPosOrVel = true;
|
|
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
|
|
kd = 0.1;
|
|
}
|
|
btScalar desiredPosition = 0.f;
|
|
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0)
|
|
{
|
|
hasDesiredPosOrVel = true;
|
|
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
|
|
kp = 0.1;
|
|
}
|
|
|
|
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];
|
|
}
|
|
|
|
motor->setVelocityTarget(desiredVelocity,kd);
|
|
motor->setPositionTarget(desiredPosition,kp);
|
|
|
|
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);
|
|
}
|
|
numMotors++;
|
|
}
|
|
|
|
}
|
|
velIndex += mb->getLink(link).m_dofCount;
|
|
posIndex += mb->getLink(link).m_posVarCount;
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
b3Warning("m_controlMode not implemented yet");
|
|
break;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
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);
|
|
|
|
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;
|
|
int totalDegreeOfFreedomU = 0;
|
|
|
|
|
|
//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());
|
|
|
|
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];
|
|
|
|
|
|
|
|
//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
|
|
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
|
|
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];
|
|
|
|
//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];
|
|
}
|
|
|
|
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();
|
|
|
|
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];
|
|
|
|
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];
|
|
}
|
|
|
|
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
|
|
|
|
if (supportsJointMotor(mb,l))
|
|
{
|
|
|
|
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
|
|
|
|
if (motor && m_data->m_physicsDeltaTime>btScalar(0))
|
|
{
|
|
btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime;
|
|
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] =
|
|
force;
|
|
//if (force>0)
|
|
//{
|
|
// b3Printf("force = %f\n", force);
|
|
//}
|
|
}
|
|
}
|
|
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
|
|
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
|
|
|
|
btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
|
|
btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
|
|
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX();
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY();
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ();
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x();
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y();
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z();
|
|
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w();
|
|
|
|
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();
|
|
|
|
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();
|
|
|
|
}
|
|
|
|
|
|
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
|
|
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
|
|
|
|
hasStatus = true;
|
|
|
|
} 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
|
|
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
|
|
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];
|
|
|
|
//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:
|
|
{
|
|
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("Step simulation request");
|
|
b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
|
|
}
|
|
///todo(erwincoumans) move this damping inside Bullet
|
|
for (int i=0;i<m_data->m_bodyHandles.size();i++)
|
|
{
|
|
applyJointDamping(i);
|
|
}
|
|
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
|
|
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
|
|
{
|
|
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
|
|
{
|
|
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
|
|
}
|
|
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
|
|
{
|
|
m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation;
|
|
}
|
|
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]);
|
|
}
|
|
|
|
}
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
|
hasStatus = true;
|
|
break;
|
|
|
|
};
|
|
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);
|
|
btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] &&
|
|
clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
|
|
clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
|
|
|
|
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)
|
|
{
|
|
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]);
|
|
|
|
mb->setBaseOmega(btVector3(0,0,0));
|
|
btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
|
|
clientCmd.m_initPoseArgs.m_initialStateQ[4],
|
|
clientCmd.m_initPoseArgs.m_initialStateQ[5],
|
|
clientCmd.m_initPoseArgs.m_initialStateQ[6]);
|
|
|
|
mb->setWorldToBaseRot(invOrn.inverse());
|
|
}
|
|
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
|
|
{
|
|
int dofIndex = 7;
|
|
for (int i=0;i<mb->getNumLinks();i++)
|
|
{
|
|
if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1))
|
|
{
|
|
mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]);
|
|
mb->setJointVel(i,0);
|
|
}
|
|
dofIndex += mb->getLink(i).m_dofCount;
|
|
}
|
|
}
|
|
}
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
|
hasStatus = true;
|
|
|
|
break;
|
|
}
|
|
|
|
|
|
case CMD_RESET_SIMULATION:
|
|
{
|
|
//clean up all data
|
|
deleteCachedInverseDynamicsBodies();
|
|
|
|
if (m_data && m_data->m_guiHelper)
|
|
{
|
|
m_data->m_guiHelper->removeAllGraphicsInstances();
|
|
}
|
|
if (m_data)
|
|
{
|
|
m_data->m_visualConverter.resetAll();
|
|
}
|
|
|
|
deleteDynamicsWorld();
|
|
createEmptyDynamicsWorld();
|
|
|
|
m_data->exitHandles();
|
|
m_data->initHandles();
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
|
|
hasStatus = true;
|
|
m_data->m_hasGround = false;
|
|
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)
|
|
{
|
|
|
|
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;
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
|
|
bool isDynamic = (mass>0);
|
|
btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
|
|
rb->setRollingFriction(0.2);
|
|
//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);
|
|
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED;
|
|
|
|
|
|
int bodyUniqueId = m_data->allocHandle();
|
|
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
|
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
|
|
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]));
|
|
|
|
|
|
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;
|
|
}
|
|
case CMD_CALCULATE_INVERSE_DYNAMICS:
|
|
{
|
|
SharedMemoryStatus& serverCmd = serverStatusOut;
|
|
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
|
|
if (bodyHandle && bodyHandle->m_multiBody)
|
|
{
|
|
btInverseDynamics::MultiBodyTree** treePtrPtr =
|
|
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
|
|
btInverseDynamics::MultiBodyTree* tree = 0;
|
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
|
|
|
if (treePtrPtr)
|
|
{
|
|
tree = *treePtrPtr;
|
|
}
|
|
else
|
|
{
|
|
btInverseDynamics::btMultiBodyTreeCreator id_creator;
|
|
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
|
|
{
|
|
b3Error("error creating tree\n");
|
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
|
}
|
|
else
|
|
{
|
|
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
|
|
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
|
|
}
|
|
}
|
|
|
|
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];
|
|
}
|
|
|
|
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
|
|
}
|
|
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
|
|
case CMD_APPLY_EXTERNAL_FORCE:
|
|
{
|
|
if (m_data->m_verboseOutput)
|
|
{
|
|
b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
|
|
}
|
|
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);
|
|
|
|
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]);
|
|
|
|
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]);
|
|
|
|
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]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
|
hasStatus = true;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
b3Error("Unknown command encountered");
|
|
|
|
SharedMemoryStatus& serverCmd =serverStatusOut;
|
|
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
|
|
hasStatus = true;
|
|
|
|
|
|
}
|
|
};
|
|
|
|
}
|
|
}
|
|
return hasStatus;
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
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();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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;
|
|
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;
|
|
}
|
|
} else
|
|
{
|
|
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
|
|
if (multiCol && multiCol->m_multiBody)
|
|
{
|
|
|
|
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);
|
|
|
|
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
|
world->addMultiBodyConstraint(p2p);
|
|
m_data->m_pickingMultiBodyPoint2Point =p2p;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// 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
|
|
|
|
btVector3 dir = rayToWorld-rayFromWorld;
|
|
dir.normalize();
|
|
dir *= m_data->m_oldPickingDist;
|
|
|
|
btVector3 newPivotB = rayFromWorld + dir;
|
|
pickCon->setPivotB(newPivotB);
|
|
}
|
|
}
|
|
|
|
if (m_data->m_pickingMultiBodyPoint2Point)
|
|
{
|
|
//keep it at the same picking distance
|
|
|
|
|
|
btVector3 dir = rayToWorld-rayFromWorld;
|
|
dir.normalize();
|
|
dir *= m_data->m_oldPickingDist;
|
|
|
|
btVector3 newPivotB = rayFromWorld + dir;
|
|
|
|
m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
|
|
}
|
|
|
|
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;
|
|
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;
|
|
}
|
|
|
|
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
|
|
{
|
|
if (m_data->m_allowRealTimeSimulation)
|
|
{
|
|
if (!m_data->m_hasGround)
|
|
{
|
|
m_data->m_hasGround = true;
|
|
|
|
int bodyId = 0;
|
|
btAlignedObjectArray<char> bufferServerToClient;
|
|
bufferServerToClient.resize(32768);
|
|
|
|
|
|
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
|
|
}
|
|
|
|
m_data->m_dynamicsWorld->stepSimulation(dtInSec);
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|