From d59b0f05a43503221b9605aa6ba1afac1dc3bdd1 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 5 Oct 2016 16:31:48 -0700 Subject: [PATCH] expose b3PhysicsParamSetNumSolverIterations split Visual Studio project generation batch file in regular and VR+pybullet+double precision build. disable KUKA iiwa joint limit hack --- build_visual_studio.bat | 8 ++---- build_visual_studio_vr_pybullet_double.bat | 8 ++++++ data/kuka_iiwa/model.urdf | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 ++ .../PhysicsServerCommandProcessor.cpp | 28 +++++++++++-------- 6 files changed, 39 insertions(+), 19 deletions(-) create mode 100644 build_visual_studio_vr_pybullet_double.bat diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 302194631..d8a85bc44 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -1,9 +1,5 @@ -IF NOT EXIST bin mkdir bin -IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin cd build3 -premake4 --enable_openvr --targetdir="../bin" vs2010 - -rem premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010 -pause +premake4 --targetdir="../bin" vs2010 +start vs2010 diff --git a/build_visual_studio_vr_pybullet_double.bat b/build_visual_studio_vr_pybullet_double.bat new file mode 100644 index 000000000..71d676172 --- /dev/null +++ b/build_visual_studio_vr_pybullet_double.bat @@ -0,0 +1,8 @@ + +IF NOT EXIST bin mkdir bin +IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin + +cd build3 + +premake4 --double --enable_openvr --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --targetdir="../bin" vs2010 +start vs2010 diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf index c3ef25fd6..a6e53d3ab 100644 --- a/data/kuka_iiwa/model.urdf +++ b/data/kuka_iiwa/model.urdf @@ -174,7 +174,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 95babb3a6..0e75054cc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -145,6 +145,15 @@ int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandH return 0; } +int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_numSolverIterations = numSolverIterations; + command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + return 0; +} + int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index dcd68c7d6..b44b9a452 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -101,6 +101,9 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); +int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); + + b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3a68fcc69..8628f8bee 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1490,7 +1490,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } - case CMD_SEND_DESIRED_STATE: + case CMD_SEND_DESIRED_STATE: { if (m_data->m_verboseOutput) { @@ -1670,18 +1670,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } default: - { - b3Warning("m_controlMode not implemented yet"); - break; - } + { + b3Warning("m_controlMode not implemented yet"); + break; + } - } - } + } + } - serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; - hasStatus = true; - break; - } + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; + hasStatus = true; + break; + } case CMD_REQUEST_ACTUAL_STATE: { if (m_data->m_verboseOutput) @@ -1944,6 +1944,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) { m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; @@ -3351,7 +3355,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { btScalar desiredVelocity = 0.f; btScalar desiredPosition = q_new[link]; - motor->setRhsClamp(gRhsClamp); + //motor->setRhsClamp(gRhsClamp); //printf("link %d: %f", link, q_new[link]); motor->setVelocityTarget(desiredVelocity,1.0); motor->setPositionTarget(desiredPosition,0.6);