mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
Merge remote-tracking branch 'bp/master' into pullRequest
This commit is contained in:
Jie
commit
88b440886d
Binary file not shown.
@ -55,7 +55,7 @@ public:
|
||||
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
|
||||
virtual bool keyboardCallback(int key, int state)=0;
|
||||
|
||||
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {}
|
||||
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]) {}
|
||||
virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
|
||||
virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]){}
|
||||
virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]){}
|
||||
|
||||
@ -122,6 +122,8 @@ SET(ExtendedTutorialsSources
|
||||
../ExtendedTutorials/SimpleBox.h
|
||||
../ExtendedTutorials/MultipleBoxes.cpp
|
||||
../ExtendedTutorials/MultipleBoxes.h
|
||||
../ExtendedTutorials/CompoundBoxes.cpp
|
||||
../ExtendedTutorials/CompoundBoxes.h
|
||||
../ExtendedTutorials/SimpleCloth.cpp
|
||||
../ExtendedTutorials/SimpleCloth.h
|
||||
../ExtendedTutorials/SimpleJoint.cpp
|
||||
|
||||
@ -71,6 +71,7 @@
|
||||
//Extended Tutorial Includes Added by Mobeen and Benelot
|
||||
#include "../ExtendedTutorials/SimpleBox.h"
|
||||
#include "../ExtendedTutorials/MultipleBoxes.h"
|
||||
#include "../ExtendedTutorials/CompoundBoxes.h"
|
||||
#include "../ExtendedTutorials/SimpleJoint.h"
|
||||
#include "../ExtendedTutorials/SimpleCloth.h"
|
||||
#include "../ExtendedTutorials/Chain.h"
|
||||
@ -322,6 +323,7 @@ static ExampleEntry gDefaultExamples[]=
|
||||
ExampleEntry(0,"Extended Tutorials"),
|
||||
ExampleEntry(1,"Simple Box", "Simplest possible demo creating a single box rigid body that falls under gravity", ET_SimpleBoxCreateFunc),
|
||||
ExampleEntry(1,"Multiple Boxes", "Add multiple box rigid bodies that fall under gravity", ET_MultipleBoxesCreateFunc),
|
||||
ExampleEntry(1,"Compound Boxes", "Add multiple boxes to a single CompoundShape to form a simple rigid L-beam, that falls under gravity", ET_CompoundBoxesCreateFunc),
|
||||
ExampleEntry(1,"Simple Joint", "Create a single distance constraint between two box rigid bodies", ET_SimpleJointCreateFunc),
|
||||
ExampleEntry(1,"Simple Cloth", "Create a simple piece of cloth", ET_SimpleClothCreateFunc),
|
||||
ExampleEntry(1,"Simple Chain", "Create a simple chain using a pair of point2point/distance constraints. You may click and drag any box to see the chain respond.", ET_ChainCreateFunc),
|
||||
|
||||
@ -377,6 +377,13 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa
|
||||
|
||||
void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
|
||||
{
|
||||
if (flag == COV_ENABLE_Y_AXIS_UP)
|
||||
{
|
||||
//either Y = up or Z
|
||||
int upAxis = enable? 1:2;
|
||||
s_app->setUpAxis(upAxis);
|
||||
}
|
||||
|
||||
if (flag == COV_ENABLE_RENDERING)
|
||||
{
|
||||
gEnableRenderLoop = (enable!=0);
|
||||
|
||||
@ -30,19 +30,16 @@ static OpenGLExampleBrowser* sExampleBrowser=0;
|
||||
static void cleanup(int signo)
|
||||
{
|
||||
|
||||
if (interrupted) { // this is the second time, we're hanging somewhere
|
||||
// if (example) {
|
||||
// example->abort();
|
||||
// }
|
||||
|
||||
if (!interrupted) { // this is the second time, we're hanging somewhere
|
||||
b3Printf("Aborting and deleting SharedMemoryCommon object");
|
||||
sleep(1);
|
||||
delete sExampleBrowser;
|
||||
sExampleBrowser = 0;
|
||||
sleep(1);
|
||||
sExampleBrowser = 0;
|
||||
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
|
||||
} else
|
||||
{
|
||||
b3Printf("no action");
|
||||
b3Printf("no action");
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
interrupted = true;
|
||||
warnx("caught signal %d", signo);
|
||||
|
||||
135
examples/ExtendedTutorials/CompoundBoxes.cpp
Normal file
135
examples/ExtendedTutorials/CompoundBoxes.cpp
Normal file
@ -0,0 +1,135 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "CompoundBoxes.h"
|
||||
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||
|
||||
|
||||
struct CompoundBoxesExample : public CommonRigidBodyBase
|
||||
{
|
||||
CompoundBoxesExample(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper)
|
||||
{
|
||||
}
|
||||
virtual ~CompoundBoxesExample(){}
|
||||
virtual void initPhysics();
|
||||
virtual void renderScene();
|
||||
void resetCamera()
|
||||
{
|
||||
float dist = 41;
|
||||
float pitch = -35;
|
||||
float yaw = 52;
|
||||
float targetPos[3]={0,0.46,0};
|
||||
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
|
||||
}
|
||||
};
|
||||
|
||||
void CompoundBoxesExample::initPhysics()
|
||||
{
|
||||
m_guiHelper->setUpAxis(1);
|
||||
|
||||
createEmptyDynamicsWorld();
|
||||
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
|
||||
if (m_dynamicsWorld->getDebugDrawer())
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
|
||||
|
||||
///create a few basic rigid bodies
|
||||
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
|
||||
m_collisionShapes.push_back(groundShape);
|
||||
|
||||
btTransform groundTransform;
|
||||
groundTransform.setIdentity();
|
||||
groundTransform.setOrigin(btVector3(0,-50,0));
|
||||
{
|
||||
btScalar mass(0.);
|
||||
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
//create a few dynamic rigidbodies
|
||||
// Re-using the same collision is better for memory usage and performance
|
||||
btBoxShape* cube = createBoxShape(btVector3(0.5,0.5,0.5));
|
||||
m_collisionShapes.push_back(cube);
|
||||
|
||||
// create a new compound shape for making an L-beam from `cube`s
|
||||
btCompoundShape* compoundShape = new btCompoundShape();
|
||||
|
||||
btTransform transform;
|
||||
|
||||
// add cubes in an L-beam fashion to the compound shape
|
||||
transform.setIdentity();
|
||||
transform.setOrigin(btVector3(0, 0, 0));
|
||||
compoundShape->addChildShape(transform, cube);
|
||||
|
||||
transform.setIdentity();
|
||||
transform.setOrigin(btVector3(0, -1, 0));
|
||||
compoundShape->addChildShape(transform, cube);
|
||||
|
||||
transform.setIdentity();
|
||||
transform.setOrigin(btVector3(0, 0, 1));
|
||||
compoundShape->addChildShape(transform, cube);
|
||||
|
||||
|
||||
btScalar masses[3]={1, 1, 1};
|
||||
btTransform principal;
|
||||
btVector3 inertia;
|
||||
compoundShape->calculatePrincipalAxisTransform(masses, principal, inertia);
|
||||
|
||||
// new compund shape to store
|
||||
btCompoundShape* compound2 = new btCompoundShape();
|
||||
m_collisionShapes.push_back(compound2);
|
||||
#if 0
|
||||
// less efficient way to add the entire compund shape
|
||||
// to a new compund shape as a child
|
||||
compound2->addChildShape(principal.inverse(), compoundShape);
|
||||
#else
|
||||
// recompute the shift to make sure the compound shape is re-aligned
|
||||
for (int i=0; i < compoundShape->getNumChildShapes(); i++)
|
||||
compound2->addChildShape(compoundShape->getChildTransform(i) * principal.inverse(),
|
||||
compoundShape->getChildShape(i));
|
||||
#endif
|
||||
delete compoundShape;
|
||||
|
||||
transform.setIdentity();
|
||||
transform.setOrigin(btVector3(0, 10, 0));
|
||||
createRigidBody(1.0, transform, compound2);
|
||||
}
|
||||
|
||||
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
||||
}
|
||||
|
||||
|
||||
void CompoundBoxesExample::renderScene()
|
||||
{
|
||||
CommonRigidBodyBase::renderScene();
|
||||
}
|
||||
|
||||
|
||||
CommonExampleInterface* ET_CompoundBoxesCreateFunc(CommonExampleOptions& options)
|
||||
{
|
||||
return new CompoundBoxesExample(options.m_guiHelper);
|
||||
}
|
||||
|
||||
|
||||
|
||||
22
examples/ExtendedTutorials/CompoundBoxes.h
Normal file
22
examples/ExtendedTutorials/CompoundBoxes.h
Normal file
@ -0,0 +1,22 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2015 Google Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#ifndef ET_COMPOUND_BOXES_EXAMPLE_H
|
||||
#define ET_COMPOUND_BOXES_EXAMPLE_H
|
||||
|
||||
class CommonExampleInterface* ET_CompoundBoxesCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
|
||||
#endif //ET_COMPOUND_BOXES_EXAMPLE_H
|
||||
@ -17,6 +17,10 @@ struct CachedObjResult
|
||||
static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
|
||||
static int gEnableFileCaching = 1;
|
||||
|
||||
int b3IsFileCachingEnabled()
|
||||
{
|
||||
return gEnableFileCaching;
|
||||
}
|
||||
void b3EnableFileCaching(int enable)
|
||||
{
|
||||
gEnableFileCaching = enable;
|
||||
|
||||
@ -6,6 +6,7 @@ struct GLInstanceGraphicsShape;
|
||||
|
||||
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
|
||||
|
||||
int b3IsFileCachingEnabled();
|
||||
void b3EnableFileCaching(int enable);
|
||||
|
||||
std::string LoadFromCachedOrFromObj(
|
||||
|
||||
@ -34,8 +34,8 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
|
||||
{
|
||||
{
|
||||
b3JointInfo info;
|
||||
info.m_jointName = 0;
|
||||
info.m_linkName = 0;
|
||||
info.m_jointName[0] = 0;
|
||||
info.m_linkName[0] = 0;
|
||||
info.m_flags = 0;
|
||||
info.m_jointIndex = link;
|
||||
info.m_qIndex =
|
||||
@ -48,14 +48,16 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
|
||||
b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
|
||||
mb->m_links[link].m_linkName);
|
||||
}
|
||||
info.m_linkName = strDup(mb->m_links[link].m_linkName);
|
||||
strcpy(info.m_linkName,mb->m_links[link].m_linkName);
|
||||
|
||||
}
|
||||
if (mb->m_links[link].m_jointName) {
|
||||
if (verboseOutput) {
|
||||
b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
|
||||
mb->m_links[link].m_jointName);
|
||||
}
|
||||
info.m_jointName = strDup(mb->m_links[link].m_jointName);
|
||||
strcpy(info.m_jointName,mb->m_links[link].m_jointName);
|
||||
//info.m_jointName = strDup(mb->m_links[link].m_jointName);
|
||||
}
|
||||
|
||||
info.m_jointType = mb->m_links[link].m_jointType;
|
||||
|
||||
@ -66,6 +66,8 @@ public:
|
||||
|
||||
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0;
|
||||
|
||||
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0;
|
||||
|
||||
virtual void setTimeOut(double timeOutInSeconds) = 0;
|
||||
virtual double getTimeOut() const = 0;
|
||||
|
||||
|
||||
@ -325,6 +325,32 @@ B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHand
|
||||
return -1;
|
||||
}
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS;
|
||||
command->m_updateFlags = 0;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
b3Assert(status);
|
||||
b3Assert(status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED);
|
||||
if (status && status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
|
||||
{
|
||||
*params = status->m_simulationParameterResultArgs;
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
@ -352,7 +378,7 @@ B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandH
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
|
||||
command->m_physSimParamArgs.m_allowRealTimeSimulation = (enableRealTimeSimulation!=0);
|
||||
command->m_physSimParamArgs.m_useRealTimeSimulation = (enableRealTimeSimulation!=0);
|
||||
command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
|
||||
return 0;
|
||||
}
|
||||
@ -395,15 +421,11 @@ B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryComman
|
||||
command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
|
||||
|
||||
command->m_physSimParamArgs.m_maxNumCmdPer1ms = maxNumCmdPer1ms;
|
||||
command->m_updateFlags |= SIM_PARAM_MAX_CMD_PER_1MS;
|
||||
//obsolete command
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching)
|
||||
@ -1438,7 +1460,10 @@ B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com
|
||||
B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
cl->disconnectSharedMemory();
|
||||
if (cl)
|
||||
{
|
||||
cl->disconnectSharedMemory();
|
||||
}
|
||||
delete cl;
|
||||
}
|
||||
|
||||
@ -1777,6 +1802,23 @@ B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle command
|
||||
}
|
||||
}
|
||||
|
||||
B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
|
||||
if (command->m_type == CMD_CUSTOM_COMMAND)
|
||||
{
|
||||
command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX;
|
||||
command->m_customCommandArgs.m_postFix[0] = 0;
|
||||
|
||||
int len = strlen(postFix);
|
||||
if (len<MAX_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_customCommandArgs.m_postFix, postFix);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle)
|
||||
@ -3377,6 +3419,48 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
|
||||
return true;
|
||||
}
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
|
||||
command->m_type = CMD_CALCULATE_MASS_MATRIX;
|
||||
command->m_updateFlags = 0;
|
||||
int numJoints = cl->getNumJoints(bodyIndex);
|
||||
for (int i = 0; i < numJoints; i++)
|
||||
{
|
||||
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
|
||||
}
|
||||
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
|
||||
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
|
||||
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
|
||||
return false;
|
||||
|
||||
if (dofCount)
|
||||
{
|
||||
*dofCount = status->m_massMatrixResultArgs.m_dofCount;
|
||||
}
|
||||
if (massMatrix)
|
||||
{
|
||||
cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
///compute the joint positions to move the end effector to a desired target using inverse kinematics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
|
||||
{
|
||||
|
||||
@ -63,6 +63,7 @@ B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
|
||||
///Plugin system, load and unload a plugin, execute a command
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient);
|
||||
B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath);
|
||||
B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix);
|
||||
B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle);
|
||||
|
||||
@ -262,14 +263,10 @@ B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandH
|
||||
B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
|
||||
B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
|
||||
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
|
||||
|
||||
B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
|
||||
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
|
||||
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
|
||||
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
|
||||
|
||||
|
||||
|
||||
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
|
||||
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
|
||||
B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
|
||||
@ -278,6 +275,9 @@ B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle
|
||||
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);
|
||||
B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params);
|
||||
|
||||
|
||||
//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
|
||||
//Use at own risk: magic things may or my not happen when calling this API
|
||||
@ -310,19 +310,22 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
|
||||
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
|
||||
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
|
||||
|
||||
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
|
||||
int* bodyUniqueId,
|
||||
int* dofCount,
|
||||
double* jointForces);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
|
||||
|
||||
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
|
||||
int* dofCount,
|
||||
double* linearJacobian,
|
||||
double* angularJacobian);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
|
||||
///the mass matrix is stored in column-major layout of size dofCount*dofCount
|
||||
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix);
|
||||
|
||||
|
||||
///compute the joint positions to move the end effector to a desired target using inverse kinematics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
|
||||
|
||||
@ -47,7 +47,7 @@ struct PhysicsClientSharedMemoryInternalData {
|
||||
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
|
||||
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
|
||||
btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
|
||||
|
||||
btAlignedObjectArray<double> m_cachedMassMatrix;
|
||||
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
|
||||
|
||||
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
|
||||
@ -108,8 +108,8 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo&
|
||||
if (bodyJointsPtr && *bodyJointsPtr)
|
||||
{
|
||||
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
||||
info.m_baseName = bodyJoints->m_baseName.c_str();
|
||||
info.m_bodyName = bodyJoints->m_bodyName.c_str();
|
||||
strcpy(info.m_baseName,bodyJoints->m_baseName.c_str());
|
||||
strcpy(info.m_bodyName,bodyJoints->m_bodyName.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -234,16 +234,6 @@ void PhysicsClientSharedMemory::resetData()
|
||||
if (bodyJointsPtr && *bodyJointsPtr)
|
||||
{
|
||||
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
|
||||
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
|
||||
if (bodyJoints->m_jointInfo[j].m_jointName)
|
||||
{
|
||||
free(bodyJoints->m_jointInfo[j].m_jointName);
|
||||
}
|
||||
if (bodyJoints->m_jointInfo[j].m_linkName)
|
||||
{
|
||||
free(bodyJoints->m_jointInfo[j].m_linkName);
|
||||
}
|
||||
}
|
||||
delete (*bodyJointsPtr);
|
||||
}
|
||||
}
|
||||
@ -392,8 +382,8 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
|
||||
template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput)
|
||||
{
|
||||
b3JointInfo info;
|
||||
info.m_jointName = 0;
|
||||
info.m_linkName = 0;
|
||||
info.m_jointName[0] = 0;
|
||||
info.m_linkName[0] = 0;
|
||||
info.m_flags = 0;
|
||||
info.m_jointIndex = linkIndex;
|
||||
info.m_qIndex = linkIndex+7;
|
||||
@ -402,7 +392,8 @@ template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex,
|
||||
|
||||
if (con->m_typeConstraintData.m_name)
|
||||
{
|
||||
info.m_jointName = strDup(con->m_typeConstraintData.m_name);
|
||||
strcpy(info.m_jointName,con->m_typeConstraintData.m_name);
|
||||
|
||||
//info.m_linkName = strDup(con->m_typeConstraintData.m_name);
|
||||
}
|
||||
|
||||
@ -1219,6 +1210,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_CALCULATED_MASS_MATRIX_FAILED:
|
||||
{
|
||||
b3Warning("calculate mass matrix failed");
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATED_MASS_MATRIX_COMPLETED:
|
||||
{
|
||||
double* matrixData = (double*)&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
|
||||
m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount);
|
||||
for (int i=0;i<serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount;i++)
|
||||
{
|
||||
m_data->m_cachedMassMatrix[i] = matrixData[i];
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
@ -1540,6 +1550,21 @@ void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation
|
||||
}
|
||||
|
||||
|
||||
void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
|
||||
{
|
||||
int sz = dofCountCheck*dofCountCheck;
|
||||
if (sz == m_data->m_cachedMassMatrix.size())
|
||||
{
|
||||
for (int i=0;i<sz;i++)
|
||||
{
|
||||
massMatrix[i] = m_data->m_cachedMassMatrix[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
|
||||
{
|
||||
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
|
||||
|
||||
@ -76,6 +76,8 @@ public:
|
||||
|
||||
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
|
||||
|
||||
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
|
||||
|
||||
virtual void setTimeOut(double timeOutInSeconds);
|
||||
virtual double getTimeOut() const;
|
||||
|
||||
|
||||
@ -40,7 +40,7 @@ struct PhysicsDirectInternalData
|
||||
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
|
||||
|
||||
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
|
||||
|
||||
btAlignedObjectArray<double> m_cachedMassMatrix;
|
||||
int m_cachedCameraPixelsWidth;
|
||||
int m_cachedCameraPixelsHeight;
|
||||
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
|
||||
@ -112,17 +112,6 @@ void PhysicsDirect::resetData()
|
||||
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
|
||||
if (bodyJointsPtr && *bodyJointsPtr)
|
||||
{
|
||||
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
|
||||
for (int j = 0; j<bodyJoints->m_jointInfo.size(); j++) {
|
||||
if (bodyJoints->m_jointInfo[j].m_jointName)
|
||||
{
|
||||
free(bodyJoints->m_jointInfo[j].m_jointName);
|
||||
}
|
||||
if (bodyJoints->m_jointInfo[j].m_linkName)
|
||||
{
|
||||
free(bodyJoints->m_jointInfo[j].m_linkName);
|
||||
}
|
||||
}
|
||||
delete (*bodyJointsPtr);
|
||||
}
|
||||
}
|
||||
@ -962,6 +951,21 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
|
||||
b3Warning("jacobian calculation failed");
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATED_MASS_MATRIX_FAILED:
|
||||
{
|
||||
b3Warning("calculate mass matrix failed");
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATED_MASS_MATRIX_COMPLETED:
|
||||
{
|
||||
double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0];
|
||||
m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount);
|
||||
for (int i=0;i<serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount;i++)
|
||||
{
|
||||
m_data->m_cachedMassMatrix[i] = matrixData[i];
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
|
||||
{
|
||||
break;
|
||||
@ -974,6 +978,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//b3Warning("Unknown server status type");
|
||||
@ -1085,8 +1093,8 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
|
||||
if (bodyJointsPtr && *bodyJointsPtr)
|
||||
{
|
||||
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
|
||||
info.m_baseName = bodyJoints->m_baseName.c_str();
|
||||
info.m_bodyName = bodyJoints->m_bodyName.c_str();
|
||||
strcpy(info.m_baseName,bodyJoints->m_baseName.c_str());
|
||||
strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -1231,6 +1239,18 @@ void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHit
|
||||
raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
|
||||
}
|
||||
|
||||
void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
|
||||
{
|
||||
int sz = dofCountCheck*dofCountCheck;
|
||||
if (sz == m_data->m_cachedMassMatrix.size())
|
||||
{
|
||||
for (int i=0;i<sz;i++)
|
||||
{
|
||||
massMatrix[i] = m_data->m_cachedMassMatrix[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PhysicsDirect::setTimeOut(double timeOutInSeconds)
|
||||
{
|
||||
m_data->m_timeOutInSeconds = timeOutInSeconds;
|
||||
|
||||
@ -99,6 +99,8 @@ public:
|
||||
|
||||
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
|
||||
|
||||
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
|
||||
|
||||
//the following APIs are for internal use for visualization:
|
||||
virtual bool connect(struct GUIHelperInterface* guiHelper);
|
||||
virtual void renderScene();
|
||||
|
||||
@ -205,6 +205,11 @@ void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastH
|
||||
return m_data->m_physicsClient->getCachedRaycastHits(raycastHits);
|
||||
}
|
||||
|
||||
void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
|
||||
{
|
||||
m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck,massMatrix);
|
||||
}
|
||||
|
||||
void PhysicsLoopBack::setTimeOut(double timeOutInSeconds)
|
||||
{
|
||||
m_data->m_physicsClient->setTimeOut(timeOutInSeconds);
|
||||
|
||||
@ -80,6 +80,8 @@ public:
|
||||
|
||||
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
|
||||
|
||||
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
|
||||
|
||||
virtual void setTimeOut(double timeOutInSeconds);
|
||||
virtual double getTimeOut() const;
|
||||
};
|
||||
|
||||
@ -40,6 +40,11 @@
|
||||
#include "../Utils/b3Clock.h"
|
||||
#include "b3PluginManager.h"
|
||||
|
||||
|
||||
#ifdef STATIC_LINK_VR_PLUGIN
|
||||
#include "plugins/vrSyncPlugin/vrSyncPlugin.h"
|
||||
#endif
|
||||
|
||||
#ifdef B3_ENABLE_TINY_AUDIO
|
||||
#include "../TinyAudio/b3SoundEngine.h"
|
||||
#endif
|
||||
@ -355,7 +360,7 @@ struct CommandLogger
|
||||
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
|
||||
{
|
||||
fwrite((const char*)&command.m_updateFlags,sizeof(int), 1,m_file);
|
||||
fwrite((const char*)&command.m_physSimParamArgs, sizeof(SendPhysicsSimulationParameters), 1,m_file);
|
||||
fwrite((const char*)&command.m_physSimParamArgs, sizeof(b3PhysicsSimulationParameters), 1,m_file);
|
||||
break;
|
||||
}
|
||||
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
|
||||
@ -559,7 +564,7 @@ struct CommandLogPlayback
|
||||
cmd->m_physSimParamArgs = unused.m_physSimParamArgs;
|
||||
#else
|
||||
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
|
||||
fread(&cmd->m_physSimParamArgs ,sizeof(SendPhysicsSimulationParameters),1,m_file);
|
||||
fread(&cmd->m_physSimParamArgs ,sizeof(b3PhysicsSimulationParameters),1,m_file);
|
||||
|
||||
#endif
|
||||
result = true;
|
||||
@ -884,8 +889,18 @@ struct b3VRControllerEvents
|
||||
if (vrEvents[i].m_numMoveEvents)
|
||||
{
|
||||
m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis;
|
||||
for (int a=0;a<10;a++)
|
||||
{
|
||||
m_vrEvents[controlledId].m_auxAnalogAxis[a] = vrEvents[i].m_auxAnalogAxis[a];
|
||||
}
|
||||
} else
|
||||
{
|
||||
m_vrEvents[controlledId].m_analogAxis = 0;
|
||||
for (int a=0;a<10;a++)
|
||||
{
|
||||
m_vrEvents[controlledId].m_auxAnalogAxis[a] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents)
|
||||
{
|
||||
m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId;
|
||||
@ -1435,7 +1450,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
|
||||
b3PluginManager m_pluginManager;
|
||||
|
||||
bool m_allowRealTimeSimulation;
|
||||
bool m_useRealTimeSimulation;
|
||||
|
||||
|
||||
b3VRControllerEvents m_vrControllerEvents;
|
||||
@ -1519,7 +1534,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
|
||||
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
|
||||
:m_pluginManager(proc),
|
||||
m_allowRealTimeSimulation(false),
|
||||
m_useRealTimeSimulation(false),
|
||||
m_commandLogger(0),
|
||||
m_logPlayback(0),
|
||||
m_physicsDeltaTime(1./240.),
|
||||
@ -1546,10 +1561,11 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
|
||||
|
||||
{
|
||||
//test to statically link a plugin
|
||||
//#include "plugins/testPlugin/testplugin.h"
|
||||
//register static plugins:
|
||||
//m_pluginManager.registerStaticLinkedPlugin("path", initPlugin, exitPlugin, executePluginCommand);
|
||||
#ifdef STATIC_LINK_VR_PLUGIN
|
||||
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin,preTickPluginCallback_vrSyncPlugin,0);
|
||||
#endif //STATIC_LINK_VR_PLUGIN
|
||||
|
||||
}
|
||||
|
||||
m_vrControllerEvents.init();
|
||||
@ -1699,7 +1715,6 @@ void logCallback(btDynamicsWorld *world, btScalar timeStep)
|
||||
|
||||
bool isPreTick = false;
|
||||
proc->tickPlugins(timeStep, isPreTick);
|
||||
|
||||
}
|
||||
|
||||
bool MyContactAddedCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
|
||||
@ -1810,6 +1825,11 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
|
||||
void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick)
|
||||
{
|
||||
m_data->m_pluginManager.tickPlugins(timeStep, isPreTick);
|
||||
if (!isPreTick)
|
||||
{
|
||||
//clear events after each postTick, so we don't receive events multiple ticks
|
||||
m_data->m_pluginManager.clearEvents();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -2993,7 +3013,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
|
||||
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
|
||||
{
|
||||
// BT_PROFILE("processCommand");
|
||||
|
||||
|
||||
bool hasStatus = false;
|
||||
|
||||
{
|
||||
@ -5680,6 +5700,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS:
|
||||
{
|
||||
SharedMemoryStatus& serverCmd =serverStatusOut;
|
||||
serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;
|
||||
serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
|
||||
serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
|
||||
serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
|
||||
serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
|
||||
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
|
||||
serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled();
|
||||
serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;
|
||||
btVector3 grav = m_data->m_dynamicsWorld->getGravity();
|
||||
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0];
|
||||
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1];
|
||||
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
|
||||
serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
|
||||
serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps;
|
||||
serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
|
||||
serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
|
||||
serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
|
||||
serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation;
|
||||
serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
|
||||
{
|
||||
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
|
||||
@ -5690,7 +5737,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
|
||||
{
|
||||
m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation;
|
||||
m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0);
|
||||
}
|
||||
|
||||
//see
|
||||
@ -6707,6 +6754,61 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATE_MASS_MATRIX:
|
||||
{
|
||||
BT_PROFILE("CMD_CALCULATE_MASS_MATRIX");
|
||||
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
|
||||
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
|
||||
if (bodyHandle && bodyHandle->m_multiBody)
|
||||
{
|
||||
|
||||
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
|
||||
|
||||
if (tree)
|
||||
{
|
||||
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
|
||||
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
|
||||
const int totDofs = numDofs + baseDofs;
|
||||
btInverseDynamics::vecx q(totDofs);
|
||||
btInverseDynamics::matxx massMatrix(totDofs, totDofs);
|
||||
for (int i = 0; i < numDofs; i++)
|
||||
{
|
||||
q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
|
||||
}
|
||||
if (-1 != tree->calculateMassMatrix(q, &massMatrix))
|
||||
{
|
||||
serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
|
||||
// Fill in the result into the shared memory.
|
||||
double* sharedBuf = (double*)bufferServerToClient;
|
||||
int sizeInBytes = totDofs*totDofs*sizeof(double);
|
||||
if (sizeInBytes < bufferSizeInBytes)
|
||||
{
|
||||
for (int i = 0; i < (totDofs); ++i)
|
||||
{
|
||||
for (int j = 0; j < (totDofs); ++j)
|
||||
{
|
||||
int element = (totDofs) * i + j;
|
||||
|
||||
sharedBuf[element] = massMatrix(i,j);
|
||||
}
|
||||
}
|
||||
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
|
||||
}
|
||||
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_APPLY_EXTERNAL_FORCE:
|
||||
{
|
||||
BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE");
|
||||
@ -8027,7 +8129,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
|
||||
{
|
||||
//pluginPath could be registered or load from disk
|
||||
int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath);
|
||||
const char* postFix = "";
|
||||
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX)
|
||||
{
|
||||
postFix = clientCmd.m_customCommandArgs.m_postFix;
|
||||
}
|
||||
|
||||
int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix);
|
||||
if (pluginUniqueId>=0)
|
||||
{
|
||||
serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
|
||||
@ -8280,18 +8388,18 @@ double gSubStep = 0.f;
|
||||
|
||||
void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
|
||||
{
|
||||
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
|
||||
m_data->m_useRealTimeSimulation = enableRealTimeSim;
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
|
||||
{
|
||||
return m_data->m_allowRealTimeSimulation;
|
||||
return m_data->m_useRealTimeSimulation;
|
||||
}
|
||||
|
||||
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
|
||||
{
|
||||
m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
|
||||
|
||||
m_data->m_pluginManager.addEvents(vrControllerEvents, numVRControllerEvents, keyEvents, numKeyEvents, mouseEvents, numMouseEvents);
|
||||
|
||||
for (int i=0;i<m_data->m_stateLoggers.size();i++)
|
||||
{
|
||||
@ -8390,7 +8498,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const
|
||||
}
|
||||
}
|
||||
|
||||
if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
|
||||
if ((m_data->m_useRealTimeSimulation) && m_data->m_guiHelper)
|
||||
{
|
||||
|
||||
|
||||
|
||||
@ -1330,7 +1330,7 @@ public:
|
||||
btVector3 getRayTo(int x,int y);
|
||||
|
||||
virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]);
|
||||
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis);
|
||||
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]);
|
||||
virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]);
|
||||
virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]);
|
||||
|
||||
@ -2874,7 +2874,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
|
||||
}
|
||||
|
||||
|
||||
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
|
||||
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis, float auxAnalogAxes[10])
|
||||
{
|
||||
|
||||
if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS)
|
||||
@ -2927,6 +2927,11 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis;
|
||||
for (int i=0;i<10;i++)
|
||||
{
|
||||
m_args[0].m_vrControllerEvents[controllerId].m_auxAnalogAxis[i] = auxAnalogAxes[i];
|
||||
}
|
||||
|
||||
m_args[0].m_csGUI->unlock();
|
||||
|
||||
}
|
||||
|
||||
@ -114,6 +114,7 @@ enum CustomCommandEnum
|
||||
CMD_CUSTOM_COMMAND_LOAD_PLUGIN=1,
|
||||
CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN=2,
|
||||
CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND=4,
|
||||
CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX=8,
|
||||
};
|
||||
|
||||
struct b3CustomCommand
|
||||
@ -121,6 +122,8 @@ struct b3CustomCommand
|
||||
int m_pluginUniqueId;
|
||||
b3PluginArguments m_arguments;
|
||||
char m_pluginPath[MAX_FILENAME_LENGTH];
|
||||
char m_postFix[MAX_FILENAME_LENGTH];
|
||||
|
||||
};
|
||||
|
||||
struct b3CustomCommandResultArgs
|
||||
@ -425,25 +428,6 @@ enum EnumSimParamInternalSimFlags
|
||||
|
||||
///Controlling a robot involves sending the desired state to its joint motor controllers.
|
||||
///The control mode determines the state variables used for motor control.
|
||||
struct SendPhysicsSimulationParameters
|
||||
{
|
||||
double m_deltaTime;
|
||||
double m_gravityAcceleration[3];
|
||||
int m_numSimulationSubSteps;
|
||||
int m_numSolverIterations;
|
||||
bool m_allowRealTimeSimulation;
|
||||
int m_useSplitImpulse;
|
||||
double m_splitImpulsePenetrationThreshold;
|
||||
double m_contactBreakingThreshold;
|
||||
int m_maxNumCmdPer1ms;
|
||||
int m_internalSimFlags;
|
||||
double m_defaultContactERP;
|
||||
int m_collisionFilterMode;
|
||||
int m_enableFileCaching;
|
||||
double m_restitutionVelocityThreshold;
|
||||
double m_defaultNonContactERP;
|
||||
double m_frictionERP;
|
||||
};
|
||||
|
||||
struct LoadBunnyArgs
|
||||
{
|
||||
@ -638,6 +622,17 @@ struct CalculateJacobianResultArgs
|
||||
double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
|
||||
};
|
||||
|
||||
struct CalculateMassMatrixArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
|
||||
};
|
||||
|
||||
struct CalculateMassMatrixResultArgs
|
||||
{
|
||||
int m_dofCount;
|
||||
};
|
||||
|
||||
enum EnumCalculateInverseKinematicsFlags
|
||||
{
|
||||
IK_HAS_TARGET_POSITION=1,
|
||||
@ -957,7 +952,7 @@ struct SharedMemoryCommand
|
||||
struct ChangeDynamicsInfoArgs m_changeDynamicsInfoArgs;
|
||||
struct GetDynamicsInfoArgs m_getDynamicsInfoArgs;
|
||||
struct InitPoseArgs m_initPoseArgs;
|
||||
struct SendPhysicsSimulationParameters m_physSimParamArgs;
|
||||
struct b3PhysicsSimulationParameters m_physSimParamArgs;
|
||||
struct BulletDataStreamArgs m_dataStreamArguments;
|
||||
struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
|
||||
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
|
||||
@ -969,6 +964,7 @@ struct SharedMemoryCommand
|
||||
struct ExternalForceArgs m_externalForceArguments;
|
||||
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
|
||||
struct CalculateJacobianArgs m_calculateJacobianArguments;
|
||||
struct CalculateMassMatrixArgs m_calculateMassMatrixArguments;
|
||||
struct b3UserConstraint m_userConstraintArguments;
|
||||
struct RequestContactDataArgs m_requestContactPointArguments;
|
||||
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
|
||||
@ -1043,6 +1039,7 @@ struct SharedMemoryStatus
|
||||
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
|
||||
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
|
||||
struct CalculateJacobianResultArgs m_jacobianResultArgs;
|
||||
struct CalculateMassMatrixResultArgs m_massMatrixResultArgs;
|
||||
struct SendContactDataArgs m_sendContactPointArgs;
|
||||
struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
|
||||
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
|
||||
@ -1063,7 +1060,7 @@ struct SharedMemoryStatus
|
||||
struct SendMouseEvents m_sendMouseEvents;
|
||||
struct b3LoadTextureResultArgs m_loadTextureResultArguments;
|
||||
struct b3CustomCommandResultArgs m_customCommandResultArgs;
|
||||
|
||||
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
|
||||
///my convention is year/month/day/rev
|
||||
|
||||
#define SHARED_MEMORY_MAGIC_NUMBER 201709260
|
||||
#define SHARED_MEMORY_MAGIC_NUMBER 201710050
|
||||
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
|
||||
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
|
||||
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
|
||||
@ -43,6 +43,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_CALCULATE_INVERSE_DYNAMICS,
|
||||
CMD_CALCULATE_INVERSE_KINEMATICS,
|
||||
CMD_CALCULATE_JACOBIAN,
|
||||
CMD_CALCULATE_MASS_MATRIX,
|
||||
CMD_USER_CONSTRAINT,
|
||||
CMD_REQUEST_CONTACT_POINT_INFORMATION,
|
||||
CMD_REQUEST_RAY_CAST_INTERSECTIONS,
|
||||
@ -74,6 +75,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_CHANGE_TEXTURE,
|
||||
CMD_SET_ADDITIONAL_SEARCH_PATH,
|
||||
CMD_CUSTOM_COMMAND,
|
||||
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
|
||||
//don't go beyond this command!
|
||||
CMD_MAX_CLIENT_COMMANDS,
|
||||
|
||||
@ -120,6 +122,8 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
|
||||
CMD_CALCULATED_JACOBIAN_COMPLETED,
|
||||
CMD_CALCULATED_JACOBIAN_FAILED,
|
||||
CMD_CALCULATED_MASS_MATRIX_COMPLETED,
|
||||
CMD_CALCULATED_MASS_MATRIX_FAILED,
|
||||
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
|
||||
CMD_CONTACT_POINT_INFORMATION_FAILED,
|
||||
CMD_REQUEST_AABB_OVERLAP_COMPLETED,
|
||||
@ -171,7 +175,7 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_CHANGE_TEXTURE_COMMAND_FAILED,
|
||||
CMD_CUSTOM_COMMAND_COMPLETED,
|
||||
CMD_CUSTOM_COMMAND_FAILED,
|
||||
|
||||
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
@ -214,8 +218,8 @@ enum b3JointInfoFlags
|
||||
|
||||
struct b3JointInfo
|
||||
{
|
||||
char* m_linkName;
|
||||
char* m_jointName;
|
||||
char m_linkName[1024];
|
||||
char m_jointName[1024];
|
||||
int m_jointType;
|
||||
int m_qIndex;
|
||||
int m_uIndex;
|
||||
@ -254,8 +258,8 @@ struct b3UserConstraint
|
||||
|
||||
struct b3BodyInfo
|
||||
{
|
||||
const char* m_baseName;
|
||||
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
|
||||
char m_baseName[1024];
|
||||
char m_bodyName[1024]; // for btRigidBody, it does not have a base, but can still have a body name from urdf
|
||||
};
|
||||
|
||||
struct b3DynamicsInfo
|
||||
@ -335,6 +339,7 @@ enum b3VREventType
|
||||
VR_GENERIC_TRACKER_MOVE_EVENT=8,
|
||||
};
|
||||
|
||||
#define MAX_VR_ANALOG_AXIS 5
|
||||
#define MAX_VR_BUTTONS 64
|
||||
#define MAX_VR_CONTROLLERS 8
|
||||
|
||||
@ -376,7 +381,7 @@ struct b3VRControllerEvent
|
||||
float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
|
||||
|
||||
float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT
|
||||
|
||||
float m_auxAnalogAxis[MAX_VR_ANALOG_AXIS*2];//store x,y per axis, only valid if VR_CONTROLLER_MOVE_EVENT
|
||||
int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo
|
||||
};
|
||||
|
||||
@ -572,6 +577,7 @@ enum b3ConfigureDebugVisualizerEnum
|
||||
COV_ENABLE_SYNC_RENDERING_INTERNAL,
|
||||
COV_ENABLE_KEYBOARD_SHORTCUTS,
|
||||
COV_ENABLE_MOUSE_PICKING,
|
||||
COV_ENABLE_Y_AXIS_UP,
|
||||
};
|
||||
|
||||
enum b3AddUserDebugItemEnum
|
||||
@ -633,9 +639,26 @@ struct b3PluginArguments
|
||||
int m_numInts;
|
||||
int m_ints[B3_MAX_PLUGIN_ARG_SIZE];
|
||||
int m_numFloats;
|
||||
int m_floats[B3_MAX_PLUGIN_ARG_SIZE];
|
||||
|
||||
double m_floats[B3_MAX_PLUGIN_ARG_SIZE];
|
||||
};
|
||||
|
||||
struct b3PhysicsSimulationParameters
|
||||
{
|
||||
double m_deltaTime;
|
||||
double m_gravityAcceleration[3];
|
||||
int m_numSimulationSubSteps;
|
||||
int m_numSolverIterations;
|
||||
int m_useRealTimeSimulation;
|
||||
int m_useSplitImpulse;
|
||||
double m_splitImpulsePenetrationThreshold;
|
||||
double m_contactBreakingThreshold;
|
||||
int m_internalSimFlags;
|
||||
double m_defaultContactERP;
|
||||
int m_collisionFilterMode;
|
||||
int m_enableFileCaching;
|
||||
double m_restitutionVelocityThreshold;
|
||||
double m_defaultNonContactERP;
|
||||
double m_frictionERP;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -74,8 +74,11 @@ typedef b3PoolBodyHandle<b3Plugin> b3PluginHandle;
|
||||
struct b3PluginManagerInternalData
|
||||
{
|
||||
b3ResizablePool<b3PluginHandle> m_plugins;
|
||||
b3HashMap<b3HashString, b3PluginHandle> m_pluginMap;
|
||||
b3HashMap<b3HashString, b3PluginHandle*> m_pluginMap;
|
||||
PhysicsDirect* m_physicsDirect;
|
||||
b3AlignedObjectArray<b3KeyboardEvent> m_keyEvents;
|
||||
b3AlignedObjectArray<b3VRControllerEvent> m_vrEvents;
|
||||
b3AlignedObjectArray<b3MouseEvent> m_mouseEvents;
|
||||
};
|
||||
|
||||
b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk)
|
||||
@ -89,8 +92,8 @@ b3PluginManager::~b3PluginManager()
|
||||
{
|
||||
while (m_data->m_pluginMap.size())
|
||||
{
|
||||
b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(0);
|
||||
unloadPlugin(plugin->m_pluginUniqueId);
|
||||
b3PluginHandle** plugin = m_data->m_pluginMap.getAtIndex(0);
|
||||
unloadPlugin((*plugin)->m_pluginUniqueId);
|
||||
}
|
||||
delete m_data->m_physicsDirect;
|
||||
m_data->m_pluginMap.clear();
|
||||
@ -98,16 +101,40 @@ b3PluginManager::~b3PluginManager()
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
void b3PluginManager::addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
|
||||
{
|
||||
|
||||
int b3PluginManager::loadPlugin(const char* pluginPath)
|
||||
for (int i = 0; i < numKeyEvents; i++)
|
||||
{
|
||||
m_data->m_keyEvents.push_back(keyEvents[i]);
|
||||
}
|
||||
|
||||
for (int i = 0; i < numVRControllerEvents; i++)
|
||||
{
|
||||
m_data->m_vrEvents.push_back(vrControllerEvents[i]);
|
||||
}
|
||||
for (int i = 0; i < numMouseEvents; i++)
|
||||
{
|
||||
m_data->m_mouseEvents.push_back(mouseEvents[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void b3PluginManager::clearEvents()
|
||||
{
|
||||
m_data->m_keyEvents.resize(0);
|
||||
m_data->m_vrEvents.resize(0);
|
||||
m_data->m_mouseEvents.resize(0);
|
||||
}
|
||||
|
||||
int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
|
||||
{
|
||||
int pluginUniqueId = -1;
|
||||
|
||||
b3Plugin* pluginOrg = m_data->m_pluginMap.find(pluginPath);
|
||||
if (pluginOrg)
|
||||
b3PluginHandle** pluginOrgPtr = m_data->m_pluginMap.find(pluginPath);
|
||||
if (pluginOrgPtr)
|
||||
{
|
||||
//already loaded
|
||||
pluginUniqueId = pluginOrg->m_pluginUniqueId;
|
||||
pluginUniqueId = (*pluginOrgPtr)->m_pluginUniqueId;
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -118,12 +145,18 @@ int b3PluginManager::loadPlugin(const char* pluginPath)
|
||||
bool ok = false;
|
||||
if (pluginHandle)
|
||||
{
|
||||
std::string postFix = postFixStr;
|
||||
std::string initStr = std::string("initPlugin") + postFix;
|
||||
std::string exitStr = std::string("exitPlugin") + postFix;
|
||||
std::string executePluginCommandStr = std::string("executePluginCommand") + postFix;
|
||||
std::string preTickPluginCallbackStr = std::string("preTickPluginCallback") + postFix;
|
||||
std::string postTickPluginCallback = std::string("postTickPluginCallback") + postFix;
|
||||
|
||||
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, "initPlugin");
|
||||
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, "exitPlugin");
|
||||
plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, "executePluginCommand");
|
||||
plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "preTickPluginCallback");
|
||||
plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "postTickPluginCallback");
|
||||
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str());
|
||||
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str());
|
||||
plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, executePluginCommandStr.c_str());
|
||||
plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, preTickPluginCallbackStr.c_str());
|
||||
plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, postTickPluginCallback.c_str());
|
||||
|
||||
if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
|
||||
{
|
||||
@ -140,7 +173,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath)
|
||||
plugin->m_ownsPluginHandle = true;
|
||||
plugin->m_pluginHandle = pluginHandle;
|
||||
plugin->m_pluginPath = pluginPath;
|
||||
m_data->m_pluginMap.insert(pluginPath, *plugin);
|
||||
m_data->m_pluginMap.insert(pluginPath, plugin);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -190,14 +223,28 @@ void b3PluginManager::tickPlugins(double timeStep, bool isPreTick)
|
||||
{
|
||||
for (int i=0;i<m_data->m_pluginMap.size();i++)
|
||||
{
|
||||
b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(i);
|
||||
|
||||
b3PluginHandle** pluginPtr = m_data->m_pluginMap.getAtIndex(i);
|
||||
b3PluginHandle* plugin = 0;
|
||||
if (pluginPtr && *pluginPtr)
|
||||
{
|
||||
plugin = *pluginPtr;
|
||||
}
|
||||
else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
PFN_TICK tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc;
|
||||
if (tick)
|
||||
{
|
||||
b3PluginContext context;
|
||||
context.m_userPointer = plugin->m_userPointer;
|
||||
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
|
||||
context.m_numMouseEvents = m_data->m_mouseEvents.size();
|
||||
context.m_mouseEvents = m_data->m_mouseEvents.size() ? &m_data->m_mouseEvents[0] : 0;
|
||||
context.m_numKeyEvents = m_data->m_keyEvents.size();
|
||||
context.m_keyEvents = m_data->m_keyEvents.size() ? &m_data->m_keyEvents[0] : 0;
|
||||
context.m_numVRControllerEvents = m_data->m_vrEvents.size();
|
||||
context.m_vrControllerEvents = m_data->m_vrEvents.size()? &m_data->m_vrEvents[0]:0;
|
||||
int result = tick(&context);
|
||||
plugin->m_userPointer = context.m_userPointer;
|
||||
}
|
||||
@ -214,7 +261,13 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
|
||||
b3PluginContext context;
|
||||
context.m_userPointer = plugin->m_userPointer;
|
||||
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
|
||||
|
||||
context.m_numMouseEvents = 0;
|
||||
context.m_mouseEvents = 0;
|
||||
context.m_numKeyEvents = 0;
|
||||
context.m_keyEvents = 0;
|
||||
context.m_numVRControllerEvents = 0;
|
||||
context.m_vrControllerEvents = 0;
|
||||
|
||||
result = plugin->m_executeCommandFunc(&context, arguments);
|
||||
plugin->m_userPointer = context.m_userPointer;
|
||||
}
|
||||
@ -242,12 +295,19 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
|
||||
pluginHandle->m_pluginPath = pluginPath;
|
||||
pluginHandle->m_userPointer = 0;
|
||||
|
||||
m_data->m_pluginMap.insert(pluginPath, *pluginHandle);
|
||||
m_data->m_pluginMap.insert(pluginPath, pluginHandle);
|
||||
|
||||
{
|
||||
b3PluginContext context;
|
||||
context.m_userPointer = 0;
|
||||
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
|
||||
context.m_numMouseEvents = 0;
|
||||
context.m_mouseEvents = 0;
|
||||
context.m_numKeyEvents = 0;
|
||||
context.m_keyEvents = 0;
|
||||
context.m_numVRControllerEvents = 0;
|
||||
context.m_vrControllerEvents = 0;
|
||||
|
||||
int result = pluginHandle->m_initFunc(&context);
|
||||
pluginHandle->m_userPointer = context.m_userPointer;
|
||||
}
|
||||
|
||||
@ -12,10 +12,14 @@ class b3PluginManager
|
||||
b3PluginManager(class PhysicsCommandProcessorInterface* physSdk);
|
||||
virtual ~b3PluginManager();
|
||||
|
||||
int loadPlugin(const char* pluginPath);
|
||||
int loadPlugin(const char* pluginPath, const char* postFixStr ="");
|
||||
void unloadPlugin(int pluginUniqueId);
|
||||
int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments);
|
||||
void addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
|
||||
void clearEvents();
|
||||
|
||||
void tickPlugins(double timeStep, bool isPreTick);
|
||||
|
||||
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc);
|
||||
|
||||
};
|
||||
|
||||
@ -5,12 +5,17 @@
|
||||
|
||||
struct b3PluginContext
|
||||
{
|
||||
|
||||
b3PhysicsClientHandle m_physClient;
|
||||
|
||||
//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
|
||||
void* m_userPointer;
|
||||
|
||||
const struct b3VRControllerEvent* m_vrControllerEvents;
|
||||
int m_numVRControllerEvents;
|
||||
const struct b3KeyboardEvent* m_keyEvents;
|
||||
int m_numKeyEvents;
|
||||
const struct b3MouseEvent* m_mouseEvents;
|
||||
int m_numMouseEvents;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -37,7 +37,7 @@ struct MyClass
|
||||
}
|
||||
};
|
||||
|
||||
B3_SHARED_API int initPlugin(struct b3PluginContext* context)
|
||||
B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context)
|
||||
{
|
||||
MyClass* obj = new MyClass();
|
||||
context->m_userPointer = obj;
|
||||
@ -47,28 +47,17 @@ B3_SHARED_API int initPlugin(struct b3PluginContext* context)
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
|
||||
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context)
|
||||
{
|
||||
MyClass* obj = (MyClass* )context->m_userPointer;
|
||||
if (obj->m_controllerId>=0)
|
||||
if (obj && obj->m_controllerId>=0)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient);
|
||||
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
|
||||
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
|
||||
int statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
|
||||
{
|
||||
struct b3VREventsData vrEvents;
|
||||
|
||||
int i = 0;
|
||||
b3GetVREventsData(context->m_physClient, &vrEvents);
|
||||
if (vrEvents.m_numControllerEvents)
|
||||
{
|
||||
for (int n=0;n<vrEvents.m_numControllerEvents;n++)
|
||||
for (int n=0;n<context->m_numVRControllerEvents;n++)
|
||||
{
|
||||
b3VRControllerEvent& event = vrEvents.m_controllerEvents[n];
|
||||
const b3VRControllerEvent& event = context->m_vrControllerEvents[n];
|
||||
if (event.m_controllerId ==obj->m_controllerId)
|
||||
{
|
||||
if (obj->m_constraintId>=0)
|
||||
@ -171,7 +160,7 @@ B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
|
||||
|
||||
|
||||
|
||||
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
|
||||
B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
|
||||
{
|
||||
MyClass* obj = (MyClass*) context->m_userPointer;
|
||||
if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2)
|
||||
@ -199,7 +188,7 @@ B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const st
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
|
||||
B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context)
|
||||
{
|
||||
MyClass* obj = (MyClass*) context->m_userPointer;
|
||||
delete obj;
|
||||
|
||||
@ -9,12 +9,12 @@ extern "C"
|
||||
#endif
|
||||
|
||||
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
|
||||
B3_SHARED_API int initPlugin(struct b3PluginContext* context);
|
||||
B3_SHARED_API void exitPlugin(struct b3PluginContext* context);
|
||||
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
|
||||
B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context);
|
||||
B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context);
|
||||
B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
|
||||
|
||||
//preTickPluginCallback and postTickPluginCallback are optional.
|
||||
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
|
||||
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -376,10 +376,16 @@ void MyKeyboardCallback(int key, int state)
|
||||
extern bool useShadowMap;
|
||||
static bool gEnableVRRenderControllers=true;
|
||||
static bool gEnableVRRendering = true;
|
||||
|
||||
static int gUpAxis = 2;
|
||||
|
||||
void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable)
|
||||
{
|
||||
if (flag == COV_ENABLE_Y_AXIS_UP)
|
||||
{
|
||||
//either Y = up or Z
|
||||
gUpAxis = enable? 1:2;
|
||||
}
|
||||
|
||||
if (flag == COV_ENABLE_SHADOWS)
|
||||
{
|
||||
useShadowMap = enable;
|
||||
@ -808,7 +814,15 @@ bool CMainApplication::HandleInput()
|
||||
for (int button = 0; button < vr::k_EButton_Max; button++)
|
||||
{
|
||||
uint64_t trigger = vr::ButtonMaskFromId((vr::EVRButtonId)button);
|
||||
|
||||
|
||||
btAssert(vr::k_unControllerStateAxisCount>=5);
|
||||
float allAxis[10];//store x,y times 5 controllers
|
||||
int index=0;
|
||||
for (int i=0;i<5;i++)
|
||||
{
|
||||
allAxis[index++]=state.rAxis[i].x;
|
||||
allAxis[index++]=state.rAxis[i].y;
|
||||
}
|
||||
bool isTrigger = (state.ulButtonPressed&trigger) != 0;
|
||||
if (isTrigger)
|
||||
{
|
||||
@ -818,31 +832,15 @@ bool CMainApplication::HandleInput()
|
||||
if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0)
|
||||
{
|
||||
// printf("Device PRESSED: %d, button %d\n", unDevice, button);
|
||||
if (button==2)
|
||||
{
|
||||
//glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
|
||||
|
||||
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
|
||||
///so it can (and likely will) cause crashes
|
||||
///add a special debug drawer that deals with this
|
||||
//gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+
|
||||
//btIDebugDraw::DBG_DrawConstraintLimits+
|
||||
//btIDebugDraw::DBG_DrawConstraints
|
||||
//;
|
||||
//gDebugDrawFlags = btIDebugDraw::DBG_DrawFrames;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
|
||||
// printf("Device MOVED: %d\n", unDevice);
|
||||
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
|
||||
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x, allAxis);
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -865,7 +863,7 @@ bool CMainApplication::HandleInput()
|
||||
} else
|
||||
{
|
||||
|
||||
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
|
||||
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x,allAxis);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -890,6 +888,7 @@ void CMainApplication::RunMainLoop()
|
||||
|
||||
while ( !bQuit && !m_app->m_window->requestedExit())
|
||||
{
|
||||
this->m_app->setUpAxis(gUpAxis);
|
||||
b3ChromeUtilsEnableProfiling();
|
||||
if (gEnableVRRendering)
|
||||
{
|
||||
|
||||
@ -8,7 +8,9 @@ import pybullet as p
|
||||
CONTROLLER_ID = 0
|
||||
POSITION=1
|
||||
ORIENTATION=2
|
||||
NUM_MOVE_EVENTS=5
|
||||
BUTTONS=6
|
||||
ANALOG_AXIS=8
|
||||
|
||||
#assume that the VR physics server is already started before
|
||||
c = p.connect(p.SHARED_MEMORY)
|
||||
@ -20,7 +22,7 @@ p.setInternalSimFlags(0)#don't load default robot assets etc
|
||||
p.resetSimulation()
|
||||
p.loadURDF("plane.urdf")
|
||||
|
||||
prevPosition=[None]*p.VR_MAX_CONTROLLERS
|
||||
prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS
|
||||
colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
|
||||
widths = [3]*p.VR_MAX_CONTROLLERS
|
||||
|
||||
@ -32,10 +34,27 @@ colors[3] = [0,0,0.5]
|
||||
colors[4] = [0.5,0.5,0.]
|
||||
colors[5] = [.5,.5,.5]
|
||||
|
||||
while True:
|
||||
controllerId = -1
|
||||
pt=[0,0,0]
|
||||
|
||||
print("waiting for VR controller trigger")
|
||||
while (controllerId<0):
|
||||
events = p.getVREvents()
|
||||
for e in (events):
|
||||
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
|
||||
controllerId = e[CONTROLLER_ID]
|
||||
if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN):
|
||||
controllerId = e[CONTROLLER_ID]
|
||||
|
||||
print("Using controllerId="+str(controllerId))
|
||||
|
||||
while True:
|
||||
events = p.getVREvents(allAnalogAxes=1)
|
||||
|
||||
for e in (events):
|
||||
if (e[CONTROLLER_ID]==controllerId ):
|
||||
for a in range(10):
|
||||
print("analog axis"+str(a)+"="+str(e[8][a]))
|
||||
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
|
||||
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
|
||||
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
|
||||
@ -51,7 +70,10 @@ while True:
|
||||
pt = prevPosition[e[CONTROLLER_ID]]
|
||||
|
||||
#print(prevPosition[e[0]])
|
||||
#print(e[1])
|
||||
print("e[POSITION]")
|
||||
print(e[POSITION])
|
||||
print("pt")
|
||||
print(pt)
|
||||
diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
|
||||
lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
|
||||
ptDistThreshold = 0.01
|
||||
|
||||
@ -95,8 +95,32 @@ p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
|
||||
plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
|
||||
controllerId = 3
|
||||
|
||||
|
||||
|
||||
CONTROLLER_ID = 0
|
||||
POSITION = 1
|
||||
ORIENTATION = 2
|
||||
BUTTONS = 6
|
||||
|
||||
controllerId = -1
|
||||
|
||||
print("waiting for VR controller trigger")
|
||||
while (controllerId<0):
|
||||
events = p.getVREvents()
|
||||
for e in (events):
|
||||
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
|
||||
controllerId = e[CONTROLLER_ID]
|
||||
if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN):
|
||||
controllerId = e[CONTROLLER_ID]
|
||||
|
||||
print("Using controllerId="+str(controllerId))
|
||||
|
||||
|
||||
plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll","_vrSyncPlugin")
|
||||
#plugin = p.loadPlugin("vrSyncPlugin")
|
||||
print("PluginId="+str(plugin))
|
||||
|
||||
p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3])
|
||||
|
||||
while (1):
|
||||
|
||||
@ -503,6 +503,35 @@ void b3pybulletExitFunc(void)
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_getConnectionInfo(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
int isConnected=0;
|
||||
int method=0;
|
||||
PyObject* pylist = 0;
|
||||
PyObject* val = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
static char* kwlist[] = {"physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm != 0)
|
||||
{
|
||||
if (b3CanSubmitCommand(sm))
|
||||
{
|
||||
isConnected = 1;
|
||||
method = sPhysicsClientsGUI[physicsClientId];
|
||||
}
|
||||
}
|
||||
|
||||
val = Py_BuildValue("{s:i,s:i}","isConnected", isConnected, "connectionMethod", method);
|
||||
return val;
|
||||
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
const char* worldFileName = "";
|
||||
@ -824,6 +853,56 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getPhysicsEngineParameters(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
PyObject* val=0;
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = {"physicsClientId", NULL};
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
struct b3PhysicsSimulationParameters params;
|
||||
int statusType;
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType!=CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Couldn't get physics simulation parameters.");
|
||||
return NULL;
|
||||
}
|
||||
b3GetStatusPhysicsSimulationParameters(statusHandle,¶ms);
|
||||
|
||||
//for now, return a subset, expose more/all on request
|
||||
val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d}",
|
||||
"fixedTimeStep", params.m_deltaTime,
|
||||
"numSubSteps", params.m_numSimulationSubSteps,
|
||||
"numSolverIterations", params.m_numSolverIterations,
|
||||
"useRealTimeSimulation", params.m_useRealTimeSimulation,
|
||||
"gravityAccelerationX", params.m_gravityAcceleration[0],
|
||||
"gravityAccelerationY", params.m_gravityAcceleration[1],
|
||||
"gravityAccelerationZ", params.m_gravityAcceleration[2]
|
||||
);
|
||||
return val;
|
||||
}
|
||||
//"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP",
|
||||
//val = Py_BuildValue("{s:i,s:i}","isConnected", isConnected, "connectionMethod", method);
|
||||
|
||||
|
||||
}
|
||||
|
||||
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
@ -918,19 +997,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
||||
{
|
||||
b3PhysicsParamSetDefaultFrictionERP(command,frictionERP);
|
||||
}
|
||||
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
}
|
||||
#if 0
|
||||
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
|
||||
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz);
|
||||
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
|
||||
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
|
||||
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
|
||||
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
|
||||
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
|
||||
#endif
|
||||
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
@ -4207,9 +4277,10 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
|
||||
int statusType;
|
||||
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
|
||||
int physicsClientId = 0;
|
||||
int allAnalogAxes = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
static char* kwlist[] = {"deviceTypeFilter", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &deviceTypeFilter, &physicsClientId))
|
||||
static char* kwlist[] = {"deviceTypeFilter", "allAnalogAxes", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, &deviceTypeFilter, &allAnalogAxes, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@ -4237,7 +4308,8 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
|
||||
vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents);
|
||||
for (i = 0; i < vrEvents.m_numControllerEvents; i++)
|
||||
{
|
||||
PyObject* vrEventObj = PyTuple_New(8);
|
||||
int numFields = allAnalogAxes? 9 : 8;
|
||||
PyObject* vrEventObj = PyTuple_New(numFields);
|
||||
|
||||
PyTuple_SetItem(vrEventObj, 0, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId));
|
||||
{
|
||||
@ -4270,6 +4342,19 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
|
||||
PyTuple_SetItem(vrEventObj, 6, buttonsObj);
|
||||
}
|
||||
PyTuple_SetItem(vrEventObj, 7, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_deviceType));
|
||||
|
||||
if (allAnalogAxes)
|
||||
{
|
||||
PyObject* buttonsObj = PyTuple_New(MAX_VR_ANALOG_AXIS*2);
|
||||
int b;
|
||||
for (b = 0; b < MAX_VR_ANALOG_AXIS*2; b++)
|
||||
{
|
||||
PyObject* axisVal = PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_auxAnalogAxis[b]);
|
||||
PyTuple_SetItem(buttonsObj, b, axisVal);
|
||||
}
|
||||
PyTuple_SetItem(vrEventObj, 8, buttonsObj);
|
||||
}
|
||||
|
||||
PyTuple_SetItem(vrEventsObj, i, vrEventObj);
|
||||
}
|
||||
return vrEventsObj;
|
||||
@ -6681,13 +6766,15 @@ static PyObject* pybullet_loadPlugin(PyObject* self,
|
||||
int physicsClientId = 0;
|
||||
|
||||
char* pluginPath = 0;
|
||||
char* postFix = 0;
|
||||
|
||||
b3SharedMemoryCommandHandle command = 0;
|
||||
b3SharedMemoryStatusHandle statusHandle = 0;
|
||||
int statusType = -1;
|
||||
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
static char* kwlist[] = { "pluginPath", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &pluginPath, &physicsClientId))
|
||||
static char* kwlist[] = { "pluginPath", "postFix", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|si", kwlist, &pluginPath, &postFix, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@ -6701,6 +6788,10 @@ static PyObject* pybullet_loadPlugin(PyObject* self,
|
||||
|
||||
command = b3CreateCustomCommand(sm);
|
||||
b3CustomCommandLoadPlugin(command, pluginPath);
|
||||
if (postFix)
|
||||
{
|
||||
b3CustomCommandLoadPluginSetPostFix(command, postFix);
|
||||
}
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusPluginUniqueId(statusHandle);
|
||||
return PyInt_FromLong(statusType);
|
||||
@ -7162,8 +7253,8 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
|
||||
double* jointPositions = (double*)malloc(byteSizeJoints);
|
||||
double* jointVelocities = (double*)malloc(byteSizeJoints);
|
||||
double* jointAccelerations = (double*)malloc(byteSizeJoints);
|
||||
double* linearJacobian = (double*)malloc(3 * byteSizeJoints);
|
||||
double* angularJacobian = (double*)malloc(3 * byteSizeJoints);
|
||||
double* linearJacobian = NULL;
|
||||
double* angularJacobian = NULL;
|
||||
|
||||
pybullet_internalSetVectord(localPosition, localPoint);
|
||||
for (i = 0; i < numJoints; i++)
|
||||
@ -7261,6 +7352,101 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
/// Given an object id, joint positions, joint velocities and joint
|
||||
/// accelerations, compute the Jacobian
|
||||
static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
{
|
||||
int bodyUniqueId;
|
||||
PyObject* objPositions;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
static char* kwlist[] = {"bodyUniqueId", "objPositions", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist,
|
||||
&bodyUniqueId, &objPositions, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
int szObPos = PySequence_Size(objPositions);
|
||||
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
||||
if (numJoints && (szObPos == numJoints))
|
||||
{
|
||||
int byteSizeJoints = sizeof(double) * numJoints;
|
||||
PyObject* pyResultList;
|
||||
double* jointPositions = (double*)malloc(byteSizeJoints);
|
||||
double* massMatrix = NULL;
|
||||
int i;
|
||||
for (i = 0; i < numJoints; i++)
|
||||
{
|
||||
jointPositions[i] =
|
||||
pybullet_internalGetFloatFromSequence(objPositions, i);
|
||||
}
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle commandHandle =
|
||||
b3CalculateMassMatrixCommandInit(sm, bodyUniqueId, jointPositions);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
|
||||
{
|
||||
int dofCount;
|
||||
b3GetStatusMassMatrix(sm, statusHandle, &dofCount, NULL);
|
||||
if (dofCount)
|
||||
{
|
||||
int byteSizeDofCount = sizeof(double) * dofCount;
|
||||
pyResultList = PyTuple_New(dofCount);
|
||||
|
||||
massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
|
||||
b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
|
||||
if (massMatrix)
|
||||
{
|
||||
int r;
|
||||
for (r = 0; r < dofCount; ++r) {
|
||||
int c;
|
||||
PyObject* pyrow = PyTuple_New(dofCount);
|
||||
for (c = 0; c < dofCount; ++c) {
|
||||
int element = r * dofCount + c;
|
||||
PyTuple_SetItem(pyrow, c,
|
||||
PyFloat_FromDouble(massMatrix[element]));
|
||||
}
|
||||
PyTuple_SetItem(pyResultList, r, pyrow);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"Internal error in calculateJacobian");
|
||||
}
|
||||
}
|
||||
free(jointPositions);
|
||||
free(massMatrix);
|
||||
if (pyResultList) return pyResultList;
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"calculateMassMatrix [numJoints] needs to be "
|
||||
"positive and [joint positions] "
|
||||
"need to match the number of joints.");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyMethodDef SpamMethods[] = {
|
||||
|
||||
{"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
|
||||
@ -7272,6 +7458,10 @@ static PyMethodDef SpamMethods[] = {
|
||||
"disconnect(physicsClientId=0)\n"
|
||||
"Disconnect from the physics server."},
|
||||
|
||||
{"getConnectionInfo", (PyCFunction)pybullet_getConnectionInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"getConnectionInfo(physicsClientId=0)\n"
|
||||
"Return if a given client id is connected, and using what method."},
|
||||
|
||||
{"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS,
|
||||
"resetSimulation(physicsClientId=0)\n"
|
||||
"Reset the simulation: remove all objects and start from an empty world."},
|
||||
@ -7304,6 +7494,9 @@ static PyMethodDef SpamMethods[] = {
|
||||
{"setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS,
|
||||
"Set some internal physics engine parameter, such as cfm or erp etc."},
|
||||
|
||||
{"getPhysicsEngineParameters", (PyCFunction)pybullet_getPhysicsEngineParameters, METH_VARARGS | METH_KEYWORDS,
|
||||
"Get the current values of internal physics engine parameters"},
|
||||
|
||||
{"setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS | METH_KEYWORDS,
|
||||
"This is for experimental purposes, use at own risk, magic may or not happen"},
|
||||
|
||||
@ -7566,6 +7759,8 @@ static PyMethodDef SpamMethods[] = {
|
||||
"accelerations, compute the joint forces using Inverse Dynamics"},
|
||||
|
||||
{"calculateJacobian", (PyCFunction)pybullet_calculateJacobian, METH_VARARGS | METH_KEYWORDS,
|
||||
"linearJacobian, angularJacobian = calculateJacobian(bodyUniqueId, "
|
||||
"linkIndex, localPosition, objPositions, objVelocities, objAccelerations, physicsClientId=0)\n"
|
||||
"Compute the jacobian for a specified local position on a body and its kinematics.\n"
|
||||
"Args:\n"
|
||||
" bodyIndex - a scalar defining the unique object id.\n"
|
||||
@ -7577,6 +7772,15 @@ static PyMethodDef SpamMethods[] = {
|
||||
"Returns:\n"
|
||||
" linearJacobian - a list of the partial linear velocities of the jacobian.\n"
|
||||
" angularJacobian - a list of the partial angular velocities of the jacobian.\n"},
|
||||
|
||||
{"calculateMassMatrix", (PyCFunction)pybullet_calculateMassMatrix, METH_VARARGS | METH_KEYWORDS,
|
||||
"massMatrix = calculateMassMatrix(bodyUniqueId, objPositions, physicsClientId=0)\n"
|
||||
"Compute the mass matrix for an object and its chain of bodies.\n"
|
||||
"Args:\n"
|
||||
" bodyIndex - a scalar defining the unique object id.\n"
|
||||
" objPositions - a list of the joint positions.\n"
|
||||
"Returns:\n"
|
||||
" massMatrix - a list of lists of the mass matrix components.\n"},
|
||||
|
||||
{"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics,
|
||||
METH_VARARGS | METH_KEYWORDS,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -10,78 +10,160 @@ using System;
|
||||
|
||||
public class NewBehaviourScript : MonoBehaviour {
|
||||
|
||||
|
||||
|
||||
|
||||
[DllImport("TestCppPlug.dll")]
|
||||
static extern int Add(int a, int b);
|
||||
|
||||
[DllImport("TestCppPlug.dll")]
|
||||
static extern int CallMe(Action<int> action);
|
||||
|
||||
[DllImport("TestCppPlug.dll")]
|
||||
static extern IntPtr CreateSharedAPI(int id);
|
||||
|
||||
[DllImport("TestCppPlug.dll")]
|
||||
static extern int GetMyIdPlusTen(IntPtr api);
|
||||
|
||||
[DllImport("TestCppPlug.dll")]
|
||||
static extern void DeleteSharedAPI(IntPtr api);
|
||||
|
||||
private void IWasCalled(int value)
|
||||
{
|
||||
text.text = value.ToString();
|
||||
}
|
||||
|
||||
|
||||
Text text;
|
||||
IntPtr sharedAPI;
|
||||
IntPtr pybullet;
|
||||
int m_cubeUid;
|
||||
|
||||
// Use this for initialization
|
||||
void Start () {
|
||||
text = GetComponent<Text>();
|
||||
CallMe(IWasCalled);
|
||||
sharedAPI = CreateSharedAPI(30);
|
||||
|
||||
pybullet = NativeMethods.b3ConnectPhysicsDirect();// SharedMemory(12347);
|
||||
|
||||
pybullet = NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY);
|
||||
if (NativeMethods.b3CanSubmitCommand(pybullet)==0)
|
||||
{
|
||||
pybullet = NativeMethods.b3ConnectPhysicsDirect();
|
||||
}
|
||||
|
||||
|
||||
|
||||
IntPtr cmd = NativeMethods.b3InitResetSimulationCommand(pybullet);
|
||||
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
|
||||
|
||||
{
|
||||
IntPtr command = NativeMethods.b3InitPhysicsParamCommand(pybullet);
|
||||
NativeMethods.b3PhysicsParamSetGravity(command, 0, -10, 0);
|
||||
IntPtr statusHandle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, command);
|
||||
}
|
||||
|
||||
int numBodies = NativeMethods.b3GetNumBodies(pybullet);
|
||||
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
|
||||
{
|
||||
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
|
||||
Quaternion qq = Quaternion.Euler(-90, 0, 0);
|
||||
NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w);
|
||||
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
|
||||
}
|
||||
|
||||
|
||||
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "cube.urdf");
|
||||
NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, 0, 20, 0);
|
||||
Quaternion q = Quaternion.Euler(35, 0, 0);
|
||||
NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, q.x, q.y, q.z, q.w);
|
||||
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
|
||||
m_cubeUid = NativeMethods.b3GetStatusBodyIndex(status);
|
||||
|
||||
EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
|
||||
if (statusType == EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
|
||||
if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
|
||||
{
|
||||
numBodies = NativeMethods.b3GetNumBodies(pybullet);
|
||||
text.text = numBodies.ToString();
|
||||
cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, m_cubeUid);
|
||||
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
|
||||
statusType = (EnumSharedMemoryServerStatus) NativeMethods.b3GetStatusType(status);
|
||||
|
||||
if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
b3VisualShapeInformation visuals = new b3VisualShapeInformation();
|
||||
NativeMethods.b3GetVisualShapeInformation(pybullet, ref visuals);
|
||||
System.Console.WriteLine("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes);
|
||||
System.IntPtr visualPtr = visuals.m_visualShapeData;
|
||||
|
||||
for (int s = 0; s < visuals.m_numVisualShapes; s++)
|
||||
{
|
||||
b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData));
|
||||
visualPtr = new IntPtr(visualPtr.ToInt64()+(Marshal.SizeOf(typeof(b3VisualShapeData))));
|
||||
double x = visual.m_dimensions[0];
|
||||
double y = visual.m_dimensions[1];
|
||||
double z = visual.m_dimensions[2];
|
||||
System.Console.WriteLine("visual.m_visualGeometryType = " + visual.m_visualGeometryType);
|
||||
System.Console.WriteLine("visual.m_dimensions" + x + "," + y + "," + z);
|
||||
if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH)
|
||||
{
|
||||
System.Console.WriteLine("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName);
|
||||
text.text = visual.m_meshAssetFileName;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (numBodies > 0)
|
||||
{
|
||||
|
||||
//b3BodyInfo info=new b3BodyInfo();
|
||||
//NativeMethods.b3GetBodyInfo(pybullet, 0, ref info );
|
||||
//text.text = info.m_baseName;
|
||||
b3BodyInfo info=new b3BodyInfo();
|
||||
NativeMethods.b3GetBodyInfo(pybullet, 0, ref info );
|
||||
|
||||
text.text = info.m_baseName;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
//IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr);
|
||||
}
|
||||
|
||||
public struct MyPos
|
||||
{
|
||||
public double x, y, z;
|
||||
public double qx, qy, qz, qw;
|
||||
}
|
||||
// Update is called once per frame
|
||||
void Update () {
|
||||
//if (pybullet != IntPtr.Zero)
|
||||
if (pybullet != IntPtr.Zero)
|
||||
{
|
||||
IntPtr cmd = NativeMethods.b3InitStepSimulationCommand(pybullet);
|
||||
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
|
||||
|
||||
//text.text = System.IO.Directory.GetCurrentDirectory().ToString();
|
||||
//text.text = Add(4, 5).ToString();
|
||||
//text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString();
|
||||
}
|
||||
if (m_cubeUid>=0)
|
||||
{
|
||||
IntPtr cmd_handle =
|
||||
NativeMethods.b3RequestActualStateCommandInit(pybullet, m_cubeUid);
|
||||
IntPtr status_handle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd_handle);
|
||||
|
||||
EnumSharedMemoryServerStatus status_type = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status_handle);
|
||||
|
||||
if (status_type == EnumSharedMemoryServerStatus.CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
IntPtr p = IntPtr.Zero;
|
||||
int objUid = 0;
|
||||
int numDofQ = 0;
|
||||
int numDofU = 0;
|
||||
IntPtr inertialFrame = IntPtr.Zero;
|
||||
IntPtr actualStateQ = IntPtr.Zero;
|
||||
IntPtr actualStateQdot = IntPtr.Zero;
|
||||
IntPtr joint_reaction_forces = IntPtr.Zero;
|
||||
|
||||
NativeMethods.b3GetStatusActualState(
|
||||
status_handle, ref objUid, ref numDofQ , ref numDofU,
|
||||
ref inertialFrame, ref actualStateQ,
|
||||
ref actualStateQdot , ref joint_reaction_forces);
|
||||
//Debug.Log("objUid=" + objUid.ToString());
|
||||
//Debug.Log("numDofQ=" + numDofQ.ToString());
|
||||
//Debug.Log("numDofU=" + numDofU.ToString());
|
||||
|
||||
MyPos mpos = (MyPos)Marshal.PtrToStructure(actualStateQ, typeof(MyPos));
|
||||
//Debug.Log("pos=(" + mpos.x.ToString()+","+ mpos.y.ToString()+ "," + mpos.z.ToString()+")");
|
||||
//Debug.Log("orn=(" + mpos.qx.ToString() + "," + mpos.qy.ToString() + "," + mpos.qz.ToString() + mpos.qw.ToString() + ")");
|
||||
Vector3 pos = new Vector3((float)mpos.x, (float)mpos.y, (float)mpos.z);
|
||||
Quaternion orn = new Quaternion((float)mpos.qx, (float)mpos.qy, (float)mpos.qz, (float)mpos.qw);
|
||||
Vector3 dimensions = new Vector3(1, 1, 1);
|
||||
CjLib.DebugUtil.DrawBox(pos, orn, dimensions, Color.black);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
CjLib.DebugUtil.DrawLine(new Vector3(-1, 0, 0), new Vector3(1, 0, 0), Color.red);
|
||||
CjLib.DebugUtil.DrawLine(new Vector3(0, -1, 0), new Vector3(0, 1, 0), Color.green);
|
||||
CjLib.DebugUtil.DrawLine(new Vector3(0, 0, -1), new Vector3(0, 0, 1), Color.blue);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void OnDestroy()
|
||||
{
|
||||
NativeMethods.b3DisconnectSharedMemory(pybullet);
|
||||
if (pybullet != IntPtr.Zero)
|
||||
{
|
||||
NativeMethods.b3DisconnectSharedMemory(pybullet);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,12 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 6197b3a0389e92c47b7d8698e5f61f06
|
||||
timeCreated: 1505961790
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@ -331,7 +331,7 @@ int main()
|
||||
|
||||
int majorGlVersion, minorGlVersion;
|
||||
|
||||
if (!sscanf((const char*)glGetString(GL_VERSION), "%d.%d", &majorGlVersion, &minorGlVersion)==2)
|
||||
if (!(sscanf((const char*)glGetString(GL_VERSION), "%d.%d", &majorGlVersion, &minorGlVersion)==2))
|
||||
{
|
||||
printf("Exit: Error cannot extract OpenGL version from GL_VERSION string\n");
|
||||
exit(0);
|
||||
|
||||
@ -64,7 +64,7 @@ int main(int argc, char *argv[])
|
||||
break;
|
||||
|
||||
case ENET_EVENT_TYPE_RECEIVE:
|
||||
printf("A packet of length %u containing '%s' was "
|
||||
printf("A packet of length %lu containing '%s' was "
|
||||
"received from %s on channel %u.\n",
|
||||
event.packet->dataLength,
|
||||
event.packet->data,
|
||||
@ -83,6 +83,8 @@ int main(int argc, char *argv[])
|
||||
|
||||
event.peer->data = NULL;
|
||||
|
||||
break;
|
||||
case ENET_EVENT_TYPE_NONE:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user