From 758ca025d5e9817c497e4e906392402d015ee310 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 24 Aug 2016 14:25:06 -0700 Subject: [PATCH] Set physics clock subdivider. --- examples/RoboticsLearning/GripperGraspExample.cpp | 4 ++-- examples/RoboticsLearning/b3RobotSimAPI.cpp | 9 +++++++++ examples/RoboticsLearning/b3RobotSimAPI.h | 2 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 9 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsServerCommandProcessor.cpp | 11 ++++++++++- 6 files changed, 33 insertions(+), 3 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 7db87b26a..a6086ada9 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -87,7 +87,7 @@ public: args.m_fileName = "cube_small.urdf"; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); - args.m_useMultiBody = true; + args.m_useMultiBody = false; m_robotSim.loadFile(args, results); } @@ -152,7 +152,7 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - + m_robotSim.setNumSimulationSubSteps(4); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index cf437c498..686eeecdf 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -460,6 +460,15 @@ void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration) } +void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps) +{ + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); +} + b3RobotSimAPI::~b3RobotSimAPI() { delete m_data; diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index e23153dc3..88506e871 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -95,6 +95,8 @@ public: void stepSimulation(); void setGravity(const b3Vector3& gravityAcceleration); + + void setNumSimulationSubSteps(int numSubSteps); void renderScene(); void debugDraw(int debugDrawMode); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1fbba3f61..7f144eeac 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -154,6 +154,15 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double return 0; } +int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS; + command->m_physSimParamArgs.m_numSimulationSubSteps = numSubSteps; + return 0; +} + b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 48490b582..351c87677 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -84,6 +84,7 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); +int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6b98d3b0b..9d7dcf5e5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -385,6 +385,7 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; + btScalar m_numSimulationSubSteps; btAlignedObjectArray m_multiBodyJointFeedbacks; btHashMap m_inverseDynamicsBodies; @@ -428,6 +429,7 @@ struct PhysicsServerCommandProcessorInternalData m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.), + m_numSimulationSubSteps(0), m_dynamicsWorld(0), m_remoteDebugDrawer(0), m_guiHelper(0), @@ -1812,7 +1814,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { applyJointDamping(i); } - m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); + if (m_data->m_numSimulationSubSteps > 0) + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,m_data->m_numSimulationSubSteps,m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps); + else + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; @@ -1842,6 +1847,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;