Binary file not shown.
@@ -266,9 +266,8 @@ void openFileDemo(const char* filename)
|
||||
s_parameterInterface->removeAllParameters();
|
||||
|
||||
|
||||
CommonExampleOptions options(s_guiHelper,0);
|
||||
CommonExampleOptions options(s_guiHelper,1);
|
||||
options.m_fileName = filename;
|
||||
|
||||
char fullPath[1024];
|
||||
sprintf(fullPath, "%s", filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
|
||||
@@ -13,6 +13,9 @@
|
||||
|
||||
#include "ExampleEntries.h"
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
|
||||
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
b3CommandLineArgs args(argc,argv);
|
||||
|
||||
@@ -794,8 +794,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
vis.m_localMaterial = **mat;
|
||||
} else
|
||||
{
|
||||
logger->reportError("Cannot find material with name:");
|
||||
logger->reportError(vis.m_materialName.c_str());
|
||||
//logger->reportError("Cannot find material with name:");
|
||||
//logger->reportError(vis.m_materialName.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -493,7 +493,10 @@ int stbi_write_png(char const *filename, int x, int y, int comp, const void *dat
|
||||
unsigned char *png = stbi_write_png_to_mem((unsigned char *) data, stride_bytes, x, y, comp, &len);
|
||||
if (!png) return 0;
|
||||
f = fopen(filename, "wb");
|
||||
if (!f) { free(png); return 0; }
|
||||
if (!f) {
|
||||
free(png);
|
||||
return 0;
|
||||
}
|
||||
fwrite(png, 1, len, f);
|
||||
fclose(f);
|
||||
free(png);
|
||||
|
||||
@@ -229,6 +229,9 @@ void RollingFrictionDemo::initPhysics()
|
||||
}
|
||||
|
||||
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
||||
|
||||
if (0)
|
||||
{
|
||||
btSerializer* s = new btDefaultSerializer;
|
||||
m_dynamicsWorld->serialize(s);
|
||||
b3ResourcePath p;
|
||||
@@ -239,6 +242,7 @@ void RollingFrictionDemo::initPhysics()
|
||||
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
|
||||
fclose(f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -77,38 +77,43 @@ PhysicsClientSharedMemory::PhysicsClientSharedMemory()
|
||||
|
||||
PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
|
||||
{
|
||||
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
||||
if (m_data->m_isConnected)
|
||||
{
|
||||
disconnectSharedMemory();
|
||||
}
|
||||
delete m_data->m_sharedMemory;
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void PhysicsClientSharedMemory::disconnectSharedMemory ()
|
||||
{
|
||||
if (m_data->m_isConnected)
|
||||
{
|
||||
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
||||
m_data->m_isConnected = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool PhysicsClientSharedMemory::isConnected() const
|
||||
{
|
||||
return m_data->m_isConnected ;
|
||||
}
|
||||
|
||||
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
|
||||
bool PhysicsClientSharedMemory::connect()
|
||||
{
|
||||
bool allowCreation = true;
|
||||
///server always has to create and initialize shared memory
|
||||
bool allowCreation = false;
|
||||
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
|
||||
|
||||
if (m_data->m_testBlock1)
|
||||
{
|
||||
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
|
||||
{
|
||||
if (allowSharedMemoryInitialization)
|
||||
{
|
||||
InitSharedMemoryBlock(m_data->m_testBlock1);
|
||||
b3Printf("Created and initialized shared memory block");
|
||||
m_data->m_isConnected = true;
|
||||
} else
|
||||
{
|
||||
b3Error("Error: please start server before client\n");
|
||||
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
||||
m_data->m_testBlock1 = 0;
|
||||
return false;
|
||||
}
|
||||
} else
|
||||
b3Error("Error: please start server before client\n");
|
||||
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
||||
m_data->m_testBlock1 = 0;
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
b3Printf("Connected to existing shared memory, status OK.\n");
|
||||
m_data->m_isConnected = true;
|
||||
|
||||
@@ -15,7 +15,9 @@ public:
|
||||
virtual ~PhysicsClientSharedMemory();
|
||||
|
||||
//return true if connection succesfull, can also check 'isConnected'
|
||||
virtual bool connect(bool allowSharedMemoryInitialization = true);
|
||||
virtual bool connect();
|
||||
|
||||
virtual void disconnectSharedMemory ();
|
||||
|
||||
virtual bool isConnected() const;
|
||||
|
||||
|
||||
@@ -191,10 +191,11 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c
|
||||
}
|
||||
|
||||
|
||||
b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization)
|
||||
b3PhysicsClientHandle b3ConnectSharedMemory()
|
||||
{
|
||||
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
|
||||
cl->connect(allowSharedMemoryInitialization);
|
||||
///client should never create shared memory, only the server does
|
||||
cl->connect();
|
||||
return (b3PhysicsClientHandle ) cl;
|
||||
}
|
||||
|
||||
|
||||
@@ -12,7 +12,8 @@ B3_DECLARE_HANDLE(b3PhysicsRobotHandle);
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
b3PhysicsClientHandle b3ConnectSharedMemory(int allowSharedMemoryInitialization);
|
||||
///make sure to start the server first!
|
||||
b3PhysicsClientHandle b3ConnectSharedMemory();
|
||||
|
||||
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
|
||||
|
||||
|
||||
@@ -9,6 +9,15 @@
|
||||
#include "PhysicsClient.h"
|
||||
#include "SharedMemoryCommands.h"
|
||||
|
||||
struct MyMotorInfo2
|
||||
{
|
||||
btScalar m_velTarget;
|
||||
btScalar m_maxForce;
|
||||
int m_uIndex;
|
||||
};
|
||||
|
||||
#define MAX_NUM_MOTORS 128
|
||||
|
||||
class PhysicsClientExample : public SharedMemoryCommon
|
||||
{
|
||||
protected:
|
||||
@@ -22,6 +31,11 @@ protected:
|
||||
|
||||
public:
|
||||
|
||||
//@todo, add accessor methods
|
||||
MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
|
||||
int m_numMotors;
|
||||
|
||||
|
||||
PhysicsClientExample(GUIHelperInterface* helper);
|
||||
virtual ~PhysicsClientExample();
|
||||
|
||||
@@ -42,11 +56,20 @@ public:
|
||||
{
|
||||
return m_wantsTermination;
|
||||
}
|
||||
|
||||
virtual bool isConnected()
|
||||
{
|
||||
return m_physicsClient.isConnected();
|
||||
}
|
||||
|
||||
void enqueueCommand(const SharedMemoryCommand& orgCommand);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void MyCallback(int buttonId, bool buttonState, void* userPtr)
|
||||
{
|
||||
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
|
||||
@@ -58,8 +81,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
|
||||
case CMD_LOAD_URDF:
|
||||
{
|
||||
command.m_type =CMD_LOAD_URDF;
|
||||
sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf");
|
||||
command.m_urdfArguments.m_initialPosition[0] = 0.0;
|
||||
sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");
|
||||
command.m_urdfArguments.m_initialPosition[0] = 0.0;
|
||||
command.m_urdfArguments.m_initialPosition[1] = 0.0;
|
||||
command.m_urdfArguments.m_initialPosition[2] = 0.0;
|
||||
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
|
||||
@@ -92,36 +115,29 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
|
||||
|
||||
case CMD_SEND_DESIRED_STATE:
|
||||
{
|
||||
command.m_type =CMD_SEND_DESIRED_STATE;
|
||||
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
|
||||
command.m_type =CMD_SEND_DESIRED_STATE;
|
||||
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
|
||||
|
||||
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
|
||||
//todo: expose a drop box in the GUI for this
|
||||
switch (controlMode)
|
||||
{
|
||||
case CONTROL_MODE_VELOCITY:
|
||||
{
|
||||
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
|
||||
{
|
||||
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 1;
|
||||
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_TORQUE:
|
||||
{
|
||||
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
|
||||
{
|
||||
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
|
||||
|
||||
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
|
||||
{
|
||||
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
|
||||
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
|
||||
}
|
||||
for (int i=0;i<cl->m_numMotors;i++)
|
||||
{
|
||||
btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
|
||||
|
||||
int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
|
||||
if (targetVel>1)
|
||||
{
|
||||
printf("testme");
|
||||
}
|
||||
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
|
||||
|
||||
}
|
||||
|
||||
cl->enqueueCommand(command);
|
||||
break;
|
||||
}
|
||||
@@ -200,7 +216,7 @@ void PhysicsClientExample::initPhysics()
|
||||
|
||||
if (!m_physicsClient.connect())
|
||||
{
|
||||
b3Warning("Cannot eonnect to physics client");
|
||||
b3Warning("Cannot connect to physics client");
|
||||
}
|
||||
|
||||
}
|
||||
@@ -222,6 +238,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
|
||||
}
|
||||
|
||||
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
|
||||
{
|
||||
for (int i=0;i<m_physicsClient.getNumJoints();i++)
|
||||
{
|
||||
b3JointInfo info;
|
||||
m_physicsClient.getJointInfo(i,info);
|
||||
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
|
||||
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
|
||||
{
|
||||
if (m_numMotors<MAX_NUM_MOTORS)
|
||||
{
|
||||
char motorName[1024];
|
||||
sprintf(motorName,"%s q'", info.m_jointName);
|
||||
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
|
||||
motorInfo->m_velTarget = 0.f;
|
||||
motorInfo->m_uIndex = info.m_uIndex;
|
||||
|
||||
SliderParams slider(motorName,&motorInfo->m_velTarget);
|
||||
slider.m_minVal=-4;
|
||||
slider.m_maxVal=4;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
m_numMotors++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (m_physicsClient.canSubmitCommand())
|
||||
|
||||
@@ -92,7 +92,7 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
|
||||
|
||||
|
||||
|
||||
bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper)
|
||||
bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper)
|
||||
{
|
||||
m_data->m_dynamicsWorld = dynamicsWorld;
|
||||
m_data->m_guiHelper = guiHelper;
|
||||
@@ -100,34 +100,37 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia
|
||||
bool allowCreation = true;
|
||||
bool allowConnectToExistingSharedMemory = false;
|
||||
|
||||
if (m_data->m_isConnected)
|
||||
{
|
||||
b3Warning("connectSharedMemory, while already connected");
|
||||
return m_data->m_isConnected;
|
||||
}
|
||||
|
||||
|
||||
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation);
|
||||
if (m_data->m_testBlock1)
|
||||
{
|
||||
if (!allowConnectToExistingSharedMemory || (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER))
|
||||
int magicId =m_data->m_testBlock1->m_magicId;
|
||||
b3Printf("magicId = %d\n", magicId);
|
||||
|
||||
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
|
||||
{
|
||||
if (allowSharedMemoryInitialization)
|
||||
{
|
||||
InitSharedMemoryBlock(m_data->m_testBlock1);
|
||||
b3Printf("Created and initialized shared memory block");
|
||||
m_data->m_isConnected = true;
|
||||
} else
|
||||
{
|
||||
b3Error("Error: please start server before client\n");
|
||||
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
||||
m_data->m_testBlock1 = 0;
|
||||
return false;
|
||||
}
|
||||
InitSharedMemoryBlock(m_data->m_testBlock1);
|
||||
b3Printf("Created and initialized shared memory block\n");
|
||||
m_data->m_isConnected = true;
|
||||
} else
|
||||
{
|
||||
b3Printf("Connected to existing shared memory, status OK.\n");
|
||||
m_data->m_isConnected = true;
|
||||
b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n");
|
||||
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
||||
m_data->m_testBlock1 = 0;
|
||||
m_data->m_isConnected = false;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Error("Cannot connect to shared memory");
|
||||
return false;
|
||||
m_data->m_isConnected = false;
|
||||
}
|
||||
return true;
|
||||
return m_data->m_isConnected;
|
||||
}
|
||||
|
||||
|
||||
@@ -693,7 +696,7 @@ void PhysicsServerSharedMemory::processClientCommands()
|
||||
case CMD_CREATE_BOX_COLLISION_SHAPE:
|
||||
{
|
||||
btVector3 halfExtents(1,1,1);
|
||||
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_HALF_EXTENTS)
|
||||
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
|
||||
{
|
||||
halfExtents = btVector3(
|
||||
clientCmd.m_createBoxShapeArguments.m_halfExtentsX,
|
||||
@@ -702,7 +705,7 @@ void PhysicsServerSharedMemory::processClientCommands()
|
||||
}
|
||||
btTransform startTrans;
|
||||
startTrans.setIdentity();
|
||||
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_POSITION)
|
||||
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION)
|
||||
{
|
||||
startTrans.setOrigin(btVector3(
|
||||
clientCmd.m_createBoxShapeArguments.m_initialPosition[0],
|
||||
@@ -710,8 +713,10 @@ void PhysicsServerSharedMemory::processClientCommands()
|
||||
clientCmd.m_createBoxShapeArguments.m_initialPosition[2]));
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_ORIENTATION)
|
||||
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
|
||||
{
|
||||
|
||||
b3Printf("test\n");
|
||||
startTrans.setRotation(btQuaternion(
|
||||
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
|
||||
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
|
||||
|
||||
@@ -20,7 +20,7 @@ public:
|
||||
virtual ~PhysicsServerSharedMemory();
|
||||
|
||||
//todo: implement option to allocated shared memory from client
|
||||
virtual bool connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper);
|
||||
virtual bool connectSharedMemory(class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper);
|
||||
|
||||
virtual void disconnectSharedMemory (bool deInitializeSharedMemory);
|
||||
|
||||
|
||||
@@ -24,9 +24,9 @@ class PhysicsServerExample : public SharedMemoryCommon
|
||||
{
|
||||
PhysicsServerSharedMemory m_physicsServer;
|
||||
|
||||
|
||||
bool m_wantsShutdown;
|
||||
bool m_wantsShutdown;
|
||||
|
||||
bool m_isConnected;
|
||||
|
||||
public:
|
||||
|
||||
@@ -50,11 +50,14 @@ public:
|
||||
}
|
||||
|
||||
virtual bool wantsTermination();
|
||||
virtual bool isConnected();
|
||||
|
||||
};
|
||||
|
||||
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
|
||||
:SharedMemoryCommon(helper),
|
||||
m_wantsShutdown(false)
|
||||
m_wantsShutdown(false),
|
||||
m_isConnected(false)
|
||||
{
|
||||
b3Printf("Started PhysicsServer\n");
|
||||
}
|
||||
@@ -65,6 +68,12 @@ PhysicsServerExample::~PhysicsServerExample()
|
||||
{
|
||||
bool deInitializeSharedMemory = true;
|
||||
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
|
||||
m_isConnected = false;
|
||||
}
|
||||
|
||||
bool PhysicsServerExample::isConnected()
|
||||
{
|
||||
return m_isConnected;
|
||||
}
|
||||
|
||||
void PhysicsServerExample::initPhysics()
|
||||
@@ -80,8 +89,7 @@ void PhysicsServerExample::initPhysics()
|
||||
grav[upAxis] = 0;//-9.8;
|
||||
this->m_dynamicsWorld->setGravity(grav);
|
||||
|
||||
bool allowSharedMemoryInitialization = true;
|
||||
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
|
||||
m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -40,10 +40,10 @@ struct btPointerCaster
|
||||
};
|
||||
};
|
||||
|
||||
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool /*allowCreation*/)
|
||||
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
|
||||
{
|
||||
#ifdef TEST_SHARED_MEMORY
|
||||
int flags = IPC_CREAT | 0666;
|
||||
int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
|
||||
int id = shmget((key_t) key, (size_t) size,flags);
|
||||
if (id < 0)
|
||||
{
|
||||
|
||||
@@ -71,8 +71,13 @@ public:
|
||||
}
|
||||
|
||||
virtual bool wantsTermination();
|
||||
virtual bool isConnected();
|
||||
};
|
||||
|
||||
bool RobotControlExample::isConnected()
|
||||
{
|
||||
return m_physicsClient.isConnected();
|
||||
}
|
||||
|
||||
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
|
||||
{
|
||||
@@ -131,6 +136,12 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
|
||||
case CMD_CREATE_BOX_COLLISION_SHAPE:
|
||||
{
|
||||
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
|
||||
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
|
||||
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
|
||||
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
|
||||
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
|
||||
|
||||
|
||||
cl->enqueueCommand(command);
|
||||
break;
|
||||
}
|
||||
@@ -232,6 +243,7 @@ m_numMotors(0)
|
||||
RobotControlExample::~RobotControlExample()
|
||||
{
|
||||
bool deInitializeSharedMemory = true;
|
||||
m_physicsClient.disconnectSharedMemory();
|
||||
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
|
||||
}
|
||||
|
||||
@@ -248,8 +260,7 @@ void RobotControlExample::initPhysics()
|
||||
grav[upAxis] = 0;//-9.8;
|
||||
this->m_dynamicsWorld->setGravity(grav);
|
||||
|
||||
bool allowSharedMemoryInitialization = true;
|
||||
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
|
||||
m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper);
|
||||
|
||||
if (m_guiHelper && m_guiHelper->getParameterInterface())
|
||||
{
|
||||
|
||||
@@ -12,6 +12,7 @@ public:
|
||||
}
|
||||
|
||||
virtual bool wantsTermination()=0;
|
||||
virtual bool isConnected()=0;
|
||||
};
|
||||
#endif//
|
||||
|
||||
|
||||
@@ -23,26 +23,63 @@ subject to the following restrictions:
|
||||
#include "SharedMemoryCommon.h"
|
||||
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
static SharedMemoryCommon* example = NULL;
|
||||
static bool interrupted = false;
|
||||
|
||||
#ifndef _WIN32
|
||||
#include <signal.h>
|
||||
#include <err.h>
|
||||
#include <unistd.h>
|
||||
static void cleanup(int signo)
|
||||
{
|
||||
if (interrupted) { // this is the second time, we're hanging somewhere
|
||||
// if (example) {
|
||||
// example->abort();
|
||||
// }
|
||||
b3Printf("Aborting and deleting SharedMemoryCommon object");
|
||||
sleep(1);
|
||||
delete example;
|
||||
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
|
||||
}
|
||||
interrupted = true;
|
||||
warnx("caught signal %d", signo);
|
||||
}
|
||||
#endif//_WIN32
|
||||
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
|
||||
#ifndef _WIN32
|
||||
struct sigaction action;
|
||||
memset(&action, 0x0, sizeof(action));
|
||||
action.sa_handler = cleanup;
|
||||
static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM };
|
||||
for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) {
|
||||
if (0 != sigaction(signos[ii], &action, NULL)) {
|
||||
err(EXIT_FAILURE, "signal %d", signos[ii]);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
b3CommandLineArgs args(argc, argv);
|
||||
|
||||
DummyGUIHelper noGfx;
|
||||
|
||||
CommonExampleOptions options(&noGfx);
|
||||
SharedMemoryCommon* example = 0;
|
||||
|
||||
if (args.CheckCmdLineFlag("client"))
|
||||
{
|
||||
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
|
||||
}else
|
||||
{
|
||||
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
|
||||
}
|
||||
{
|
||||
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
|
||||
}else
|
||||
{
|
||||
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
|
||||
}
|
||||
|
||||
example->initPhysics();
|
||||
while (!example->wantsTermination())
|
||||
while (example->isConnected() && !(example->wantsTermination() || interrupted))
|
||||
{
|
||||
example->stepSimulation(1.f/60.f);
|
||||
}
|
||||
|
||||
@@ -61,6 +61,9 @@ struct LWRightBody
|
||||
//Exponential map
|
||||
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
|
||||
|
||||
//btQuaternion q_w = [ sin(|w|*dt/2) * w/|w| , cos(|w|*dt/2)]
|
||||
//btQuaternion q_new = q_w * q_old;
|
||||
|
||||
btVector3 axis;
|
||||
btScalar fAngle = m_angularVelocity.length();
|
||||
//limit the angular motion
|
||||
@@ -88,6 +91,7 @@ struct LWRightBody
|
||||
predictedOrn.normalize();
|
||||
m_worldPose.m_worldOrientation = predictedOrn;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -99,13 +103,9 @@ struct LWRightBody
|
||||
|
||||
|
||||
|
||||
///quick demo showing the right-handed coordinate system and positive rotations around each axis
|
||||
class Tutorial : public CommonExampleInterface
|
||||
{
|
||||
CommonGraphicsApp* m_app;
|
||||
float m_x;
|
||||
float m_y;
|
||||
float m_z;
|
||||
int m_tutorialIndex;
|
||||
|
||||
LWRightBody* m_body;
|
||||
@@ -114,14 +114,14 @@ public:
|
||||
|
||||
Tutorial(CommonGraphicsApp* app, int tutorialIndex)
|
||||
:m_app(app),
|
||||
m_x(0),
|
||||
m_y(0),
|
||||
m_z(0),
|
||||
m_tutorialIndex(tutorialIndex)
|
||||
{
|
||||
m_app->setUpAxis(2);
|
||||
|
||||
|
||||
switch (m_tutorialIndex)
|
||||
{
|
||||
|
||||
}
|
||||
{
|
||||
|
||||
int boxId = m_app->registerCubeShape(100,100,1);
|
||||
@@ -186,10 +186,6 @@ public:
|
||||
}
|
||||
virtual void stepSimulation(float deltaTime)
|
||||
{
|
||||
m_x+=0.01f;
|
||||
m_y+=0.01f;
|
||||
m_z+=0.01f;
|
||||
//m_body->m_worldPose.m_worldPosition+= btVector3(0,0.01,0);
|
||||
//m_body->m_linearVelocity=btVector3(0,0.1,0);
|
||||
m_body->m_angularVelocity =btVector3(0,0.1,0);
|
||||
m_body->integrateTransform(deltaTime);
|
||||
|
||||
Reference in New Issue
Block a user