diff --git a/data/slope.bullet b/data/slope.bullet index eb5c34e29..df44889da 100644 Binary files a/data/slope.bullet and b/data/slope.bullet differ diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp index d952a853a..568ce4328 100644 --- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp @@ -295,7 +295,7 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere //tab->SetHeight(300); tab->SetWidth(140); - tab->SetHeight(250); + tab->SetHeight(1250); //tab->Dock(Gwen::Pos::Left); tab->Dock( Gwen::Pos::Fill ); //tab->SetMargin( Gwen::Margin( 2, 2, 2, 2 ) ); diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index b0e2a2c83..e068b409a 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -22,7 +22,7 @@ struct PhysicsClientSharedMemoryInternalData SharedMemoryBlock* m_testBlock1; btAlignedObjectArray m_robotMultiBodyData; - btAlignedObjectArray m_poweredJointInfo; + btAlignedObjectArray m_jointInfo; int m_counter; bool m_serverLoadUrdfOK; @@ -51,14 +51,14 @@ struct PhysicsClientSharedMemoryInternalData }; -int PhysicsClientSharedMemory::getNumPoweredJoints() const +int PhysicsClientSharedMemory::getNumJoints() const { - return m_data->m_poweredJointInfo.size(); + return m_data->m_jointInfo.size(); } -void PhysicsClientSharedMemory::getPoweredJointInfo(int index, PoweredJointInfo& info) const +void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const { - info = m_data->m_poweredJointInfo[index]; + info = m_data->m_jointInfo[index]; } @@ -147,7 +147,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; hasStatus = true; serverStatus = serverCmd; - + EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; //consume the command switch (serverCmd.m_type) { @@ -183,10 +183,9 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } for (int link=0;linkm_numLinks;link++) { - if ((mb->m_links[link].m_jointType == eRevoluteType)|| - (mb->m_links[link].m_jointType == ePrismaticType)) { - PoweredJointInfo info; + b3JointInfo info; + info.m_flags = 0; info.m_qIndex = qOffset; info.m_uIndex = uOffset; @@ -201,7 +200,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt info.m_jointName = mb->m_links[link].m_jointName; info.m_jointType = mb->m_links[link].m_jointType; } - m_data->m_poweredJointInfo.push_back(info); + if ((mb->m_links[link].m_jointType == eRevoluteType)|| + (mb->m_links[link].m_jointType == ePrismaticType)) + { + info.m_flags |= JOINT_HAS_MOTORIZED_POWER; + } + m_data->m_jointInfo.push_back(info); } qOffset+= mb->m_links[link].m_posVarCount; uOffset+= mb->m_links[link].m_dofCount; @@ -217,10 +221,9 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } for (int link=0;linkm_numLinks;link++) { - if ((mb->m_links[link].m_jointType == eRevoluteType)|| - (mb->m_links[link].m_jointType == ePrismaticType)) { - PoweredJointInfo info; + b3JointInfo info; + info.m_flags = 0; info.m_qIndex = qOffset; info.m_uIndex = uOffset; @@ -235,7 +238,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt info.m_jointName = mb->m_links[link].m_jointName; info.m_jointType = mb->m_links[link].m_jointType; } - m_data->m_poweredJointInfo.push_back(info); + if ((mb->m_links[link].m_jointType == eRevoluteType)|| + (mb->m_links[link].m_jointType == ePrismaticType)) + { + info.m_flags |= JOINT_HAS_MOTORIZED_POWER; + } + m_data->m_jointInfo.push_back(info); } qOffset+= mb->m_links[link].m_posVarCount; uOffset+= mb->m_links[link].m_dofCount; @@ -338,7 +346,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } default: { - b3Error("Unknown server command\n"); + b3Error("Unknown server status\n"); btAssert(0); } }; @@ -355,7 +363,11 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt { m_data->m_waitingForServer = true; } - } + } else + { + b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, + m_data->m_testBlock1->m_numProcessedServerCommands); + } return hasStatus; } @@ -368,6 +380,8 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c { ///at the moment we allow a maximum of 1 outstanding command, so we check for this //once the server processed the command and returns a status, we clear the flag "m_data->m_waitingForServer" and allow submitting the next command + btAssert(!m_data->m_waitingForServer); + if (!m_data->m_waitingForServer) { m_data->m_testBlock1->m_clientCommands[0] = command; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 4d11f91ac..29e00f19e 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -26,9 +26,9 @@ public: virtual bool submitClientCommand(const SharedMemoryCommand& command); - virtual int getNumPoweredJoints() const; + virtual int getNumJoints() const; - virtual void getPoweredJointInfo(int index, PoweredJointInfo& info) const; + virtual void getJointInfo(int index, b3JointInfo& info) const; }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index be1ec94b3..14f58cd31 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -80,6 +80,117 @@ int b3InitStepSimulationCommand(struct SharedMemoryCommand* command) } + +int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode) +{ + b3Assert(command); + command->m_type = CMD_SEND_DESIRED_STATE; + command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; + command->m_updateFlags = 0; + return 0; +} + +int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value) +{ + b3Assert(command); + command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = 1; + return 0; +} + + +int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value) +{ + b3Assert(command); + command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + return 0; +} + +int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value) +{ + b3Assert(command); + command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + return 0; +} + + +int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command) +{ + b3Assert(command); + command->m_type =CMD_REQUEST_ACTUAL_STATE; + return 0; +} + +int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command) +{ + b3Assert(command); + command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE; + command->m_updateFlags =0; + return 0; +} + +int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_POSITION; + + command->m_createBoxShapeArguments.m_initialPosition[0] = startPosX; + command->m_createBoxShapeArguments.m_initialPosition[1] = startPosY; + command->m_createBoxShapeArguments.m_initialPosition[2] = startPosZ; + return 0; +} + +int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_ORIENTATION; + + command->m_createBoxShapeArguments.m_initialOrientation[0] = startOrnX; + command->m_createBoxShapeArguments.m_initialOrientation[1] = startOrnY; + command->m_createBoxShapeArguments.m_initialOrientation[2] = startOrnZ; + command->m_createBoxShapeArguments.m_initialOrientation[3] = startOrnW; + return 0; +} + +int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ) +{ + + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; + + command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX; + command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY; + command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ; + + return 0; +} + + + +int b3CreateSensorCommandInit(struct SharedMemoryCommand* command) +{ + b3Assert(command); + command->m_type = CMD_CREATE_SENSOR; + command->m_updateFlags = 0; + command->m_createSensorArguments.m_numJointSensorChanges = 0; + return 0; +} + +int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int jointIndex, int enable) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_SENSOR); + int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; + + command->m_createSensorArguments.m_jointIndex[curIndex] = jointIndex; + command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable; + command->m_createSensorArguments.m_numJointSensorChanges++; + return 0; +} + + b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization) { PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); @@ -111,41 +222,19 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryC return (int)cl->submitClientCommand(*command); } + + int b3GetNumJoints(b3PhysicsClientHandle physClient) { PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; - return cl->getNumPoweredJoints(); + return cl->getNumJoints(); } -void b3GetPoweredJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct PoweredJointInfo* info) + +void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info) { PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; - cl->getPoweredJointInfo(linkIndex,*info); + cl->getJointInfo(linkIndex,*info); } -#if 0 -#include "SharedMemoryBlock.h" - -#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name - -B3_DECLARE_HANDLE(b3PhysicsClientHandle); - -b3PhysicsClientHandle b3ConnectSharedMemory(int memKey, int allowSharedMemoryInitialization); - -void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); - -int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status); - -int b3CanSubmitCommand(b3PhysicsClientHandle physClient); - -int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command); - -int b3GetNumPoweredJoints(b3PhysicsClientHandle physClient); - -void b3GetPoweredJointInfo(int linkIndex, struct PoweredJointInfo* info); - -int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command); -int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz); - -#endif diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 472b19a25..35a1a5801 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -22,9 +22,9 @@ int b3CanSubmitCommand(b3PhysicsClientHandle physClient); int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command); -int b3GetNumPoweredJoints(b3PhysicsClientHandle physClient); +int b3GetNumJoints(b3PhysicsClientHandle physClient); -void b3GetPoweredJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct PoweredJointInfo* info); +void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info); int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command); int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz); @@ -39,6 +39,33 @@ int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, do int b3LoadUrdfCommandSetUseMultiBody(struct SharedMemoryCommand* command, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(struct SharedMemoryCommand* command, int useFixedBase); +///Set joint control variables such as desired position/angle, desired velocity, +///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) +int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode); +//Only use when controlMode is CONTROL_MODE_VELOCITY +int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value); +int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value); +///Only use if when controlMode is CONTROL_MODE_TORQUE, +int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value); + + +///the creation of collision shapes and rigid bodies etc is likely going to change, +///but good to have a b3CreateBoxShapeCommandInit for now + +//create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1) +//after that, you can optionally adjust the initial position, orientation and size +int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command); +int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ); +int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ); + + + int b3CreateSensorCommandInit(struct SharedMemoryCommand* command); +int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable); + + +int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command); + #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 929727963..3f7e6a833 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -215,11 +215,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime) bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { - for (int i=0;i m_jointFeedbacks; + btAlignedObjectArray m_multiBodyJointFeedbacks; btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; @@ -98,11 +98,12 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia m_data->m_guiHelper = guiHelper; bool allowCreation = true; + bool allowConnectToExistingSharedMemory = false; m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation); if (m_data->m_testBlock1) { - if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) + if (!allowConnectToExistingSharedMemory || (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)) { if (allowSharedMemoryInitialization) { @@ -287,16 +288,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& } else { btAssert(0); - /* - for (int i=0;im_dynamicsWorld->getNumConstraints();i++) - { - btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); - btJointFeedback* fb = new btJointFeedback(); - m_data->m_jointFeedbacks.push_back(fb); - c->setJointFeedback(fb); - - } - */ + return true; } @@ -407,6 +399,69 @@ void PhysicsServerSharedMemory::processClientCommands() + break; + } + case CMD_CREATE_SENSOR: + { + b3Printf("Processed CMD_CREATE_SENSOR"); + + if (m_data->m_dynamicsWorld->getNumMultibodies()>0) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); + btAssert(mb); + for (int i=0;igetLink(jointIndex).m_jointFeedback) + { + b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); + } else + { + btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); + fb->m_reactionForces.setZero(); + mb->getLink(jointIndex).m_jointFeedback = fb; + m_data->m_multiBodyJointFeedbacks.push_back(fb); + }; + + } else + { + if (mb->getLink(jointIndex).m_jointFeedback) + { + m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); + delete mb->getLink(jointIndex).m_jointFeedback; + mb->getLink(jointIndex).m_jointFeedback=0; + } else + { + b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); + }; + + } + } + + } else + { + b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); + } + +#if 0 + //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody + /* + for (int i=0;im_dynamicsWorld->getNumConstraints();i++) + { + btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); + btJointFeedback* fb = new btJointFeedback(); + m_data->m_jointFeedbacks.push_back(fb); + c->setJointFeedback(fb); + + + } + */ +#endif + + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); break; } case CMD_SEND_DESIRED_STATE: @@ -448,6 +503,8 @@ void PhysicsServerSharedMemory::processClientCommands() } case CONTROL_MODE_VELOCITY: { + b3Printf("Using CONTROL_MODE_VELOCITY"); + int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque if (m_data->m_dynamicsWorld->getNumMultibodies()>0) @@ -475,7 +532,6 @@ void PhysicsServerSharedMemory::processClientCommands() dofIndex += mb->getLink(link).m_dofCount; } } - b3Printf("Using CONTROL_MODE_TORQUE with %d motors", numMotors); break; } default: @@ -544,7 +600,26 @@ void PhysicsServerSharedMemory::processClientCommands() { serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } - + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) + { + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; + } + } else + { + btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); + btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; + } } serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; @@ -617,11 +692,33 @@ void PhysicsServerSharedMemory::processClientCommands() } case CMD_CREATE_BOX_COLLISION_SHAPE: { - btVector3 halfExtents(30,30,1); + btVector3 halfExtents(1,1,1); + if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_HALF_EXTENTS) + { + halfExtents = btVector3( + clientCmd.m_createBoxShapeArguments.m_halfExtentsX, + clientCmd.m_createBoxShapeArguments.m_halfExtentsY, + clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); + } btTransform startTrans; startTrans.setIdentity(); - startTrans.setOrigin(btVector3(0,0,-4)); - + if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_POSITION) + { + startTrans.setOrigin(btVector3( + clientCmd.m_createBoxShapeArguments.m_initialPosition[0], + clientCmd.m_createBoxShapeArguments.m_initialPosition[1], + clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); + } + + if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_ORIENTATION) + { + startTrans.setRotation(btQuaternion( + clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); + } + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index 1ccb38e25..d9ec47176 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -167,8 +167,13 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) for (int i=0;im_numMotors;i++) { btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; + int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; - command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; + if (targetVel>1) + { + printf("testme"); + } + command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; } break; @@ -300,26 +305,29 @@ void RobotControlExample::stepSimulation(float deltaTime) bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { - for (int i=0;im_velTarget = 0.f; - motorInfo->m_uIndex = info.m_uIndex; - - SliderParams slider(motorName,&motorInfo->m_velTarget); - slider.m_minVal=-4; - slider.m_maxVal=4; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - m_numMotors++; - } + if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) + { + if (m_numMotorsm_velTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + + SliderParams slider(motorName,&motorInfo->m_velTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_numMotors++; + } + } } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c399fcda0..f99a9e114 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -30,10 +30,11 @@ enum EnumSharedMemoryClientCommand // CMD_DELETE_BOX_COLLISION_SHAPE, // CMD_CREATE_RIGID_BODY, // CMD_DELETE_RIGID_BODY, -// CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback for force/torque sensors + CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors +// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED CMD_INIT_POSE, CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, - CMD_SEND_DESIRED_STATE, + CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, CMD_STEP_FORWARD_SIMULATION, CMD_SHUTDOWN, @@ -42,7 +43,7 @@ enum EnumSharedMemoryClientCommand enum EnumSharedMemoryServerStatus { - CMD_SHARED_MEMORY_NOT_INITIALIZED, + CMD_SHARED_MEMORY_NOT_INITIALIZED=0, CMD_WAITING_FOR_CLIENT_COMMAND, //CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client @@ -65,8 +66,8 @@ enum EnumSharedMemoryServerStatus }; #define SHARED_MEMORY_SERVER_TEST_C -#define MAX_DEGREE_OF_FREEDOM 1024 -#define MAX_NUM_SENSORS 1024 +#define MAX_DEGREE_OF_FREEDOM 256 +#define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 enum EnumUrdfArgsUpdateFlags @@ -105,7 +106,7 @@ struct SetJointFeedbackArgs //todo: discuss and decide about control mode and combinations enum { // POSITION_CONTROL=0, - CONTROL_MODE_VELOCITY, + CONTROL_MODE_VELOCITY=0, CONTROL_MODE_TORQUE, }; @@ -173,13 +174,38 @@ struct SendActualStateArgs //actual state is only written by the server, read-only access by client is expected double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; - double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information + + //measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z] + double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM]; }; +struct CreateSensorArgs +{ + int m_bodyUniqueId; + int m_numJointSensorChanges; + int m_jointIndex[MAX_DEGREE_OF_FREEDOM]; + int m_enableJointForceSensor[MAX_DEGREE_OF_FREEDOM]; +}; typedef struct SharedMemoryCommand SharedMemoryCommand_t; +enum EnumBoxShapeFlags +{ + BOX_SHAPE_HAS_INITIAL_POSITION=1, + BOX_SHAPE_HAS_INITIAL_ORIENTATION=2, + BOX_SHAPE_HAS_HALF_EXTENTS=4 +}; +///This command will be replaced to allow arbitrary collision shape types +struct CreateBoxShapeArgs +{ + double m_halfExtentsX; + double m_halfExtentsY; + double m_halfExtentsZ; + + double m_initialPosition[3]; + double m_initialOrientation[4]; +}; struct SharedMemoryCommand { @@ -199,6 +225,8 @@ struct SharedMemoryCommand struct BulletDataStreamArgs m_dataStreamArguments; struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; + struct CreateSensorArgs m_createSensorArguments; + struct CreateBoxShapeArgs m_createBoxShapeArguments; }; }; @@ -219,15 +247,22 @@ struct SharedMemoryStatus typedef struct SharedMemoryStatus SharedMemoryStatus_t; -struct PoweredJointInfo +enum JointInfoFlags +{ + JOINT_HAS_MOTORIZED_POWER=1, +}; +struct b3JointInfo { char* m_linkName; char* m_jointName; int m_jointType; int m_qIndex; int m_uIndex; + /// + int m_flags; }; + #endif //SHARED_MEMORY_COMMANDS_H diff --git a/src/Bullet3Common/b3Logging.h b/src/Bullet3Common/b3Logging.h index 8d8ad4dcb..b302effe4 100644 --- a/src/Bullet3Common/b3Logging.h +++ b/src/Bullet3Common/b3Logging.h @@ -2,6 +2,10 @@ #ifndef B3_LOGGING_H #define B3_LOGGING_H +#ifdef __cplusplus +extern "C" { +#endif + ///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail ///You can also customize the message by uncommenting out a different line below #define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__) @@ -18,6 +22,7 @@ void b3EnterProfileZone(const char* name); void b3LeaveProfileZone(); +#ifdef __cplusplus class b3ProfileZone { @@ -34,6 +39,7 @@ public: }; #define B3_PROFILE( name ) b3ProfileZone __profile( name ) +#endif #else //B3_NO_PROFILE @@ -64,6 +70,8 @@ void b3OutputPrintfVarArgsInternal(const char *str, ...); void b3OutputWarningMessageVarArgsInternal(const char *str, ...); void b3OutputErrorMessageVarArgsInternal(const char *str, ...); - +#ifdef __cplusplus + } +#endif #endif//B3_LOGGING_H \ No newline at end of file diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/src/BulletCollision/CollisionShapes/btConvexShape.cpp index f03d0b21e..b56d72917 100644 --- a/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -48,7 +48,7 @@ btConvexShape::~btConvexShape() } -void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const +void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const { btVector3 localAxis = dir*trans.getBasis(); btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); @@ -56,12 +56,16 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc min = vtx1.dot(dir); max = vtx2.dot(dir); - + witnesPtMax = vtx2; + witnesPtMin = vtx1; + if(min>max) { btScalar tmp = min; min = max; max = tmp; + witnesPtMax = vtx1; + witnesPtMin = vtx2; } } diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.h b/src/BulletCollision/CollisionShapes/btConvexShape.h index 290cd9fd1..875f2ac19 100644 --- a/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -52,7 +52,8 @@ public: btScalar getMarginNonVirtual () const; void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; + + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; //notice that the vectors should be unit length diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 1b6e0b9b5..1f6f7aa90 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -846,8 +846,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); - - if (0) + +#if 0 { b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", @@ -860,6 +860,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[i+1].m_bottomVec[1], zeroAccSpatFrc[i+1].m_bottomVec[2]); } +#endif // //adding damping terms (only) btScalar linDampMult = 1., angDampMult = 1.; diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 65837e8b2..9a0ecc924 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -1,6 +1,8 @@ #include "SharedMemoryBlock.h" #include "PhysicsClientC_API.h" #include "SharedMemoryCommands.h" +#include "Bullet3Common/b3Logging.h" +#include struct test { @@ -8,17 +10,21 @@ struct test }; #include -#define MAX_TIMEOUT 1024 +#define MAX_TIMEOUT 1024*1024*1024 int main(int argc, char* argv[]) { int ret ,allowSharedMemoryInitialization=0; int timeout = MAX_TIMEOUT; + int sensorJointIndexLeft=-1; + int sensorJointIndexRight=-1; + const char* urdfFileName = "r2d2.urdf"; - double gravx=0, gravy=0, gravz=0; + double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; - + b3Printf("timeout = %d\n",timeout); + SharedMemoryCommand_t command; SharedMemoryStatus_t status; @@ -34,7 +40,8 @@ int main(int argc, char* argv[]) ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - + b3Printf("timeout = %d\n",timeout); + ret = b3LoadUrdfCommandInit(&command, urdfFileName); //setting the initial position, orientation and other arguments are optional @@ -46,14 +53,103 @@ int main(int argc, char* argv[]) ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - - + b3Printf("timeout = %d\n",timeout); + + int numJoints = b3GetNumJoints(sm); + for (int i=0;i=0) || (sensorJointIndexRight>=0)) + { + ret = b3CreateSensorCommandInit(&command); + if (sensorJointIndexLeft>=0) + { + ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1); + } + if(sensorJointIndexRight>=0) + { + ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1); + } + ret = b3SubmitClientCommand(sm, &command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + } + + ret = b3CreateBoxShapeCommandInit(&command); + ret = b3CreateBoxCommandSetStartPosition(&command, 0,0,-1); + ret = b3CreateBoxCommandSetStartOrientation(&command,0,0,0,1); + ret = b3CreateBoxCommandSetHalfExtents(&command, 10,10,1); + ret = b3SubmitClientCommand(sm, &command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + + + b3RequestActualStateCommandInit(&command); + ret = b3SubmitClientCommand(sm, &command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - - ret = b3InitStepSimulationCommand(&command); + int posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; + int dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; + + b3Printf("posVarCount = %d\n",posVarCount); + printf("dofCount = %d\n",dofCount); + + b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY); + for (int dofIndex=0;dofIndex 0) && b3ProcessServerStatus(sm, &status)==0) {} + + ///perform some simulation steps for testing + for (int i=0;i<100;i++) + { + ret = b3InitStepSimulationCommand(&command); + ret = b3SubmitClientCommand(sm, &command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + } + + b3RequestActualStateCommandInit(&command); + ret = b3SubmitClientCommand(sm, &command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + + if (sensorJointIndexLeft>=0) + { + b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, + status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0], + status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1], + status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]); + } + + if (sensorJointIndexRight>=0) + { + b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, + status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0], + status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1], + status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]); + } + + } b3DisconnectSharedMemory(sm); }