...
 
Commits (24)
......@@ -69,6 +69,13 @@
trigger = "midi",
description = "Use Midi controller to control parameters"
}
newoption
{
trigger = "grpc",
description = "Build GRPC server/client features for PyBullet/BulletRobotics"
}
-- _OPTIONS["midi"] = "1";
......
......@@ -478,6 +478,12 @@ struct CommonMultiBodyBase : public CommonExampleInterface
if (m_pickedConstraint)
{
m_dynamicsWorld->removeConstraint(m_pickedConstraint);
if (m_pickedBody)
{
m_pickedBody->forceActivationState(ACTIVE_TAG);
m_pickedBody->activate(true);
}
delete m_pickedConstraint;
m_pickedConstraint = 0;
m_pickedBody = 0;
......
......@@ -564,9 +564,13 @@ bool findExistingMeshFile(
return false;
}
std::string drop_it = "package://";
if (fn.substr(0, drop_it.length())==drop_it)
fn = fn.substr(drop_it.length());
std::string drop_it_pack = "package://";
std::string drop_it_model = "model://";
if (fn.substr(0, drop_it_pack.length())==drop_it_pack)
fn = fn.substr(drop_it_pack.length());
else if (fn.substr(0, drop_it_model.length())==drop_it_model)
fn = fn.substr(drop_it_model.length());
std::list<std::string> shorter;
shorter.push_back("../..");
......
......@@ -610,9 +610,10 @@ void ConvertURDF2BulletInternal(
}
} else
{
if (canSleep)
// if (canSleep)
{
if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0)
if (cache.m_bulletMultiBody->getBaseMass()==0)
//&& cache.m_bulletMultiBody->getNumDofs()==0)
{
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
......
......@@ -53,32 +53,6 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClient
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
}
else
{
command->m_urdfArguments.m_urdfFileName[0] = 0;
}
command->m_updateFlags = URDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
......@@ -310,6 +284,56 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
}
else
{
command->m_urdfArguments.m_urdfFileName[0] = 0;
}
command->m_updateFlags = URDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* urdfFileName)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
}
else
{
command->m_urdfArguments.m_urdfFileName[0] = 0;
}
command->m_updateFlags = URDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
......@@ -629,6 +653,15 @@ B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCom
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_minimumSolverIslandSize = minimumSolverIslandSize;
command->m_updateFlags |= SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
{
......@@ -711,11 +744,18 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3Physics
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
return b3InitStepSimulationCommand2((b3SharedMemoryCommandHandle)command);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_STEP_FORWARD_SIMULATION;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
......
......@@ -320,7 +320,8 @@ B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommand
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);
B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);
......@@ -337,13 +338,14 @@ B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* urdfFileName);
B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
......
......@@ -2367,7 +2367,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
//int maxProxies = 32768;
//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
m_data->m_solver = new btMultiBodyConstraintSolver;
......@@ -2390,6 +2392,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
gDbvtMargin = btScalar(0);
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
......@@ -3004,8 +3008,10 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling)
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int orgFlags, btScalar globalScaling)
{
//clear the LOAD_SDF_FILE=1 bit, which is reserved for internal use of loadSDF command.
int flags = orgFlags & ~1;
m_data->m_sdfRecentLoadedBodies.clear();
*bodyUniqueIdPtr = -1;
......
......@@ -468,6 +468,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
SIM_PARAM_ENABLE_SAT = 8388608,
SIM_PARAM_CONSTRAINT_SOLVER_TYPE =16777216,
SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432,
};
enum EnumLoadSoftBodyUpdateFlags
......
......@@ -898,6 +898,7 @@ struct b3PhysicsSimulationParameters
double m_contactSlop;
int m_enableSAT;
int m_constraintSolverType;
int m_minimumSolverIslandSize;
};
enum eConstraintSolverTypes
......
......@@ -21,7 +21,11 @@
typedef void* B3_DYNLIB_HANDLE;
#ifdef __APPLE__
#define B3_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL)
#else
#define B3_DYNLIB_OPEN(path) dlmopen(LM_ID_NEWLM, path, RTLD_LAZY)
#endif
#define B3_DYNLIB_CLOSE dlclose
#define B3_DYNLIB_IMPORT dlsym
#endif
......
......@@ -1353,6 +1353,10 @@ bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int li
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
b3SharedMemoryStatusHandle statusHandle;
if (args.m_activationState >= 0)
{
b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId,args.m_activationState);
}
if (args.m_mass >= 0) {
b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
}
......@@ -1714,6 +1718,15 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct
b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
}
if (args.m_constraintSolverType >= 0)
{
b3PhysicsParameterSetConstraintSolverType(command, args.m_constraintSolverType);
}
if (args.m_minimumSolverIslandSize >= 0)
{
b3PhysicsParameterSetMinimumSolverIslandSize(command, args.m_minimumSolverIslandSize);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
return true;
}
......
......@@ -268,7 +268,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameter
m_frictionERP=-1;
m_solverResidualThreshold=-1;
m_constraintSolverType = -1;
m_minimumSolverIslandSize = -1;
}
};
......@@ -285,6 +286,7 @@ struct b3RobotSimulatorChangeDynamicsArgs
double m_contactStiffness;
double m_contactDamping;
int m_frictionAnchor;
int m_activationState;
b3RobotSimulatorChangeDynamicsArgs()
: m_mass(-1),
......@@ -296,7 +298,8 @@ struct b3RobotSimulatorChangeDynamicsArgs
m_angularDamping(-1),
m_contactStiffness(-1),
m_contactDamping(-1),
m_frictionAnchor(-1)
m_frictionAnchor(-1),
m_activationState(-1)
{}
};
......
#include "ConvertGRPCBullet.h"
#include "PhysicsClientC_API.h"
#include "SharedMemoryCommands.h"
#include <memory>
#include <iostream>
#include <string>
#include <thread>
#include <grpc++/grpc++.h>
#include <grpc/support/log.h>
#include "pybullet.grpc.pb.h"
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerCompletionQueue;
using grpc::Status;
using pybullet_grpc::PyBulletCommand;
using pybullet_grpc::PyBulletStatus;
using pybullet_grpc::PyBulletAPI;
SharedMemoryCommand* convertGRPCAndSubmitCommand(PyBulletCommand& grpcCommand, SharedMemoryCommand& cmd)
{
SharedMemoryCommand* cmdPtr = 0;
if (grpcCommand.has_loadurdfcommand())
{
auto grpcCmd = grpcCommand.loadurdfcommand();
std::string fileName = grpcCmd.urdffilename();
if (fileName.length())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
b3LoadUrdfCommandInit2(commandHandle, fileName.c_str());
if (grpcCmd.has_initialposition())
{
const ::pybullet_grpc::vec3& pos = grpcCmd.initialposition();
b3LoadUrdfCommandSetStartPosition(commandHandle, pos.x(), pos.y(), pos.z());
}
if (grpcCmd.has_initialorientation())
{
const ::pybullet_grpc::quat4& orn = grpcCmd.initialorientation();
b3LoadUrdfCommandSetStartOrientation(commandHandle, orn.x(), orn.y(), orn.z(), orn.w());
}
if (grpcCmd.hasUseMultiBody_case()== ::pybullet_grpc::LoadUrdfCommand::HasUseMultiBodyCase::kUseMultiBody)
{
b3LoadUrdfCommandSetUseMultiBody( commandHandle, grpcCmd.usemultibody());
}
if (grpcCmd.hasGlobalScaling_case() == ::pybullet_grpc::LoadUrdfCommand::HasGlobalScalingCase::kGlobalScaling)
{
b3LoadUrdfCommandSetGlobalScaling(commandHandle, grpcCmd.globalscaling());
}
if (grpcCmd.hasUseFixedBase_case() == ::pybullet_grpc::LoadUrdfCommand::HasUseFixedBaseCase::kUseFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(commandHandle, grpcCmd.usefixedbase());
}
if (grpcCmd.urdfflags())
{
b3LoadUrdfCommandSetFlags(commandHandle, grpcCmd.urdfflags());
}
}
}
if (grpcCommand.has_stepsimulationcommand())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
b3InitStepSimulationCommand2(commandHandle);
}
return cmdPtr;
}
bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes, PyBulletStatus& grpcReply)
{
bool converted = false;
grpcReply.set_statustype(serverStatus.m_type);
switch (serverStatus.m_type)
{
case CMD_URDF_LOADING_COMPLETED:
{
::pybullet_grpc::LoadUrdfStatus* stat = grpcReply.mutable_urdfstatus();
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
int objectUniqueId = b3GetStatusBodyIndex(statusHandle);
stat->set_objectuniqueid(objectUniqueId);
}
break;
}
return converted;
}
#ifndef BT_CONVERT_GRPC_BULLET_H
#define BT_CONVERT_GRPC_BULLET_H
#include "../PhysicsClientC_API.h"
namespace pybullet_grpc
{
class PyBulletCommand;
class PyBulletStatus;
};
struct SharedMemoryCommand* convertGRPCAndSubmitCommand(pybullet_grpc::PyBulletCommand& grpcCommand, struct SharedMemoryCommand& cmd);
bool convertStatusToGRPC(const struct SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes, pybullet_grpc::PyBulletStatus& grpcReply);
#endif //BT_CONVERT_GRPC_BULLET_H
\ No newline at end of file
///PyBullet / BulletRobotics GRPC server.
///works as standalone GRPC server as as a GRPC server bridge,
///connecting to a local physics server using shared memory
#include <stdio.h>
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "PhysicsClientC_API.h"
#ifdef NO_SHARED_MEMORY
#include "PhysicsServerCommandProcessor.h"
typedef PhysicsServerCommandProcessor MyCommandProcessor;
#else
#include "SharedMemoryCommandProcessor.h"
typedef SharedMemoryCommandProcessor MyCommandProcessor;
#endif //NO_SHARED_MEMORY
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "PhysicsServerCommandProcessor.h"
#include "../Utils/b3Clock.h"
#include <memory>
#include <iostream>
#include <string>
#include <thread>
#include <grpc++/grpc++.h>
#include <grpc/support/log.h>
#include "pybullet.grpc.pb.h"
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerCompletionQueue;
using grpc::Status;
using pybullet_grpc::PyBulletCommand;
using pybullet_grpc::PyBulletStatus;
using pybullet_grpc::PyBulletAPI;
bool gVerboseNetworkMessagesServer = true;
#include "ConvertGRPCBullet.h"
class ServerImpl final {
public:
~ServerImpl() {
server_->Shutdown();
// Always shutdown the completion queue after the server.
cq_->Shutdown();
}
void Run(MyCommandProcessor* comProc) {
std::string server_address("0.0.0.0:50051");
ServerBuilder builder;
// Listen on the given address without any authentication mechanism.
builder.AddListeningPort(server_address, grpc::InsecureServerCredentials());
// Register "service_" as the instance through which we'll communicate with
// clients. In this case it corresponds to an *asynchronous* service.
builder.RegisterService(&service_);
// Get hold of the completion queue used for the asynchronous communication
// with the gRPC runtime.
cq_ = builder.AddCompletionQueue();
// Finally assemble the server.
server_ = builder.BuildAndStart();
std::cout << "Server listening on " << server_address << std::endl;
// Proceed to the server's main loop.
HandleRpcs(comProc);
}
private:
// Class encompasing the state and logic needed to serve a request.
class CallData {
public:
// Take in the "service" instance (in this case representing an asynchronous
// server) and the completion queue "cq" used for asynchronous communication
// with the gRPC runtime.
CallData(PyBulletAPI::AsyncService* service, ServerCompletionQueue* cq, MyCommandProcessor* comProc)
: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE) , m_finished(false), m_comProc(comProc){
// Invoke the serving logic right away.
Proceed();
}
enum CallStatus { CREATE, PROCESS, FINISH, TERMINATE };
CallStatus Proceed() {
if (status_ == CREATE) {
// Make this instance progress to the PROCESS state.
status_ = PROCESS;
// As part of the initial CREATE state, we *request* that the system
// start processing SayHello requests. In this request, "this" acts are
// the tag uniquely identifying the request (so that different CallData
// instances can serve different requests concurrently), in this case
// the memory address of this CallData instance.
service_->RequestSubmitCommand(&ctx_, &m_command, &responder_, cq_, cq_,
this);
}
else if (status_ == PROCESS) {
// Spawn a new CallData instance to serve new clients while we process
// the one for this CallData. The instance will deallocate itself as
// part of its FINISH state.
new CallData(service_, cq_, m_comProc);
status_ = FINISH;
std::string replyString;
// The actual processing.
SharedMemoryStatus serverStatus;
b3AlignedObjectArray<char> buffer;
buffer.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
SharedMemoryCommand cmd;
SharedMemoryCommand* cmdPtr = 0;
m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
cmdPtr = convertGRPCAndSubmitCommand(m_command, cmd);
if (cmdPtr)
{
bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
double timeOutInSeconds = 10;
b3Clock clock;
double startTimeSeconds = clock.getTimeInSeconds();
double curTimeSeconds = clock.getTimeInSeconds();
while ((!hasStatus) && ((curTimeSeconds - startTimeSeconds) <timeOutInSeconds))
{
hasStatus = m_comProc->receiveStatus(serverStatus, &buffer[0], buffer.size());
curTimeSeconds = clock.getTimeInSeconds();
}
if (gVerboseNetworkMessagesServer)
{
//printf("buffer.size = %d\n", buffer.size());
printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
}
if (hasStatus)
{
b3AlignedObjectArray<unsigned char> packetData;
unsigned char* statBytes = (unsigned char*)&serverStatus;
convertStatusToGRPC(serverStatus, &buffer[0], buffer.size(), m_status);
}
}
if (m_command.has_terminateservercommand())
{
status_ = TERMINATE;
}
// And we are done! Let the gRPC runtime know we've finished, using the
// memory address of this instance as the uniquely identifying tag for
// the event.
responder_.Finish(m_status, Status::OK, this);
}
else {
GPR_ASSERT(status_ == FINISH);
// Once in the FINISH state, deallocate ourselves (CallData).
delete this;
}
return status_;
}
private:
// The means of communication with the gRPC runtime for an asynchronous
// server.
PyBulletAPI::AsyncService* service_;
// The producer-consumer queue where for asynchronous server notifications.
ServerCompletionQueue* cq_;
// Context for the rpc, allowing to tweak aspects of it such as the use
// of compression, authentication, as well as to send metadata back to the
// client.
ServerContext ctx_;
// What we get from the client.
PyBulletCommand m_command;
// What we send back to the client.
PyBulletStatus m_status;
// The means to get back to the client.
ServerAsyncResponseWriter<PyBulletStatus> responder_;
// Let's implement a tiny state machine with the following states.
CallStatus status_; // The current serving state.
bool m_finished;
MyCommandProcessor* m_comProc; //physics server command processor
};
// This can be run in multiple threads if needed.
void HandleRpcs(MyCommandProcessor* comProc) {
// Spawn a new CallData instance to serve new clients.
new CallData(&service_, cq_.get(), comProc);
void* tag; // uniquely identifies a request.
bool ok;
bool finished = false;
CallData::CallStatus status = CallData::CallStatus::CREATE;
while (status!= CallData::CallStatus::TERMINATE) {
// Block waiting to read the next event from the completion queue. The
// event is uniquely identified by its tag, which in this case is the
// memory address of a CallData instance.
// The return value of Next should always be checked. This return value
// tells us whether there is any kind of event or cq_ is shutting down.
grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
{
//GPR_ASSERT(cq_->Next(&tag, &ok));
GPR_ASSERT(ok);
status = static_cast<CallData*>(tag)->Proceed();
}
}
}
std::unique_ptr<ServerCompletionQueue> cq_;
PyBulletAPI::AsyncService service_;
std::unique_ptr<Server> server_;
};
int main(int argc, char** argv)
{
b3CommandLineArgs parseArgs(argc, argv);
b3Clock clock;
double timeOutInSeconds = 10;
DummyGUIHelper guiHelper;
MyCommandProcessor* sm = new MyCommandProcessor;
sm->setGuiHelper(&guiHelper);
int port = 6667;
parseArgs.GetCmdLineArgument("port", port);
gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose");
#ifndef NO_SHARED_MEMORY
int key = 0;
if (parseArgs.GetCmdLineArgument("sharedMemoryKey", key))
{
sm->setSharedMemoryKey(key);
}
#endif//NO_SHARED_MEMORY
bool isPhysicsClientConnected = sm->connect();
bool exitRequested = false;
if (isPhysicsClientConnected)
{
ServerImpl server;
server.Run(sm);
}
else
{
printf("Couldn't connect to physics server\n");
}
delete sm;
return 0;
}
project ("App_PhysicsServerSharedMemoryBridgeGRPC")
language "C++"
kind "ConsoleApp"
includedirs {"../../ThirdPartyLibs/clsocket/src","../../../src",".."}
if os.is("Windows") then
defines { "WIN32", "_WIN32_WINNT=0x0600" }
links {"grpc","grpc++","grpc++_reflection","gpr",
"libprotobuf","crypto","ssl","zlibstaticd","Ws2_32","Winmm" }
end
if os.is("Linux") then
defines {"_LINUX"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
links {
"BulletFileLoader",
"Bullet3Common",
"LinearMath"
}
files {
"main.cpp",
"../PhysicsClient.cpp",
"../PhysicsClient.h",
"../PhysicsDirect.cpp",
"../PhysicsDirect.h",
"../PhysicsCommandProcessorInterface.h",
"../SharedMemoryCommandProcessor.cpp",
"../SharedMemoryCommandProcessor.h",
"../PhysicsClientC_API.cpp",
"../PhysicsClientC_API.h",
"../Win32SharedMemory.cpp",
"../Win32SharedMemory.h",
"../PosixSharedMemory.cpp",
"../PosixSharedMemory.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3ResourcePath.h",
"../../Utils/b3Clock.cpp",
"../../Utils/b3Clock.h",
}
project "App_PhysicsServerGRPC"
if _OPTIONS["ios"] then
kind "WindowedApp"
else
kind "ConsoleApp"
end
defines { "NO_SHARED_MEMORY" }
includedirs {"..","../../../src", "../../ThirdPartyLibs","../../ThirdPartyLibs/clsocket/src"}
links {
"clsocket","Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletSoftBody", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
}
if os.is("Windows") then
defines { "WIN32", "_WIN32_WINNT=0x0600" }
links {"grpc","grpc++","grpc++_reflection","gpr",
"libprotobuf","crypto","ssl","zlibstaticd","Ws2_32","Winmm" }
end
if os.is("Linux") then
defines {"_LINUX"}
links{"dl"}
end
if os.is("MacOSX") then
defines {"_DARWIN"}
end
language "C++"
myfiles =
{
"../IKTrajectoryHelper.cpp",
"../IKTrajectoryHelper.h",
"../SharedMemoryCommands.h",
"../SharedMemoryPublic.h",
"../PhysicsServerCommandProcessor.cpp",
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClientC_API.cpp",
"../PhysicsClient.cpp",
"../plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.h",
"../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../b3RobotSimulatorClientAPI_NoDirect.h",
"../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
"../../TinyRenderer/geometry.cpp",
"../../TinyRenderer/model.cpp",
"../../TinyRenderer/tgaimage.cpp",
"../../TinyRenderer/our_gl.cpp",
"../../TinyRenderer/TinyRenderer.cpp",
"../../OpenGLWindow/SimpleCamera.cpp",
"../../OpenGLWindow/SimpleCamera.h",
"../../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
"../../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
"../../Importers/ImportURDFDemo/MyMultiBodyCreator.h",
"../../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
"../../Importers/ImportMJCFDemo/BulletMJCFImporter.h",
"../../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../../Importers/ImportURDFDemo/UrdfParser.cpp",
"../../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../Importers/ImportURDFDemo/UrdfParser.cpp",
"../../Importers/ImportURDFDemo/UrdfParser.h",
"../../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../Importers/ImportURDFDemo/URDF2Bullet.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3Clock.cpp",
"../../Utils/ChromeTraceUtil.cpp",
"../../Utils/ChromeTraceUtil.h",
"../../Utils/RobotLoggingUtil.cpp",
"../../Utils/RobotLoggingUtil.h",
"../../../Extras/Serialize/BulletWorldImporter/*",
"../../../Extras/Serialize/BulletFileLoader/*",
"../../Importers/ImportURDFDemo/URDFImporterInterface.h",
"../../Importers/ImportURDFDemo/URDFJointTypes.h",
"../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
"../../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
"../../Importers/ImportSTLDemo/ImportSTLSetup.h",
"../../Importers/ImportSTLDemo/LoadMeshFromSTL.h",
"../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../ThirdPartyLibs/stb_image/stb_image.cpp",
}
files {
myfiles,
"main.cpp",
}
del ..\pybullet.pb.cpp
del ..\pybullet.pb.h
del ..\pybullet.grpc.pb.cpp
del ..\pybullet.grpc.pb.h
..\..\..\ThirdPartyLibs\grpc\lib\win32\protoc --proto_path=. --cpp_out=. pybullet.proto
..\..\..\ThirdPartyLibs\grpc\lib\win32\protoc.exe --plugin=protoc-gen-grpc="..\..\..\ThirdPartyLibs\grpc\lib\win32\grpc_cpp_plugin.exe" --grpc_out=. pybullet.proto
move pybullet.grpc.pb.cc ..\pybullet.grpc.pb.cpp
move pybullet.grpc.pb.h ..\pybullet.grpc.pb.h
move pybullet.pb.cc ..\pybullet.pb.cpp
move pybullet.pb.h ..\pybullet.pb.h
del ..\pybullet_pb2.py
del ..\pybullet_pb2_grpc.py
..\..\..\ThirdPartyLibs\grpc\lib\win32\protoc --proto_path=. --python_out=. pybullet.proto
python -m grpc_tools.protoc -I. --python_out=. --grpc_python_out=. pybullet.proto
move pybullet_pb2.py ..\pybullet_pb2.py
move pybullet_pb2_grpc.py ..\pybullet_pb2_grpc.py
rm ../pybullet.pb.cpp
rm ../pybullet.pb.h
rm ../pybullet.grpc.pb.cpp
rm ../pybullet.grpc.pb.h
protoc --proto_path=. --cpp_out=. pybullet.proto
protoc --plugin=protoc-gen-grpc=`which grpc_cpp_plugin` --grpc_out=. pybullet.proto
mv pybullet.grpc.pb.cc ../pybullet.grpc.pb.cpp
mv pybullet.grpc.pb.h ../pybullet.grpc.pb.h
mv pybullet.pb.cc ../pybullet.pb.cpp
mv pybullet.pb.h ../pybullet.pb.h
rm ../pybullet_pb2.py
rm ../pybullet_pb2_grpc.py
protoc --proto_path=. --python_out=. pybullet.proto
python -m grpc_tools.protoc -I. --python_out=. --grpc_python_out=. pybullet.proto
mv pybullet_pb2.py ../pybullet_pb2.py
mv pybullet_pb2_grpc.py ../pybullet_pb2_grpc.py
syntax = "proto3";
option java_multiple_files = true;
option java_package = "io.grpc.pybullet_grpc";
option java_outer_classname = "PyBulletProto";
option objc_class_prefix = "PBG";
package pybullet_grpc;
service PyBulletAPI {
// Sends a greeting
rpc SubmitCommand (PyBulletCommand) returns (PyBulletStatus) {}
}
message vec3
{
double x=1;
double y=2;
double z=3;
};
message quat4
{
double x=1;
double y=2;
double z=3;
double w=4;
};
message TerminateServerCommand
{
string exitReason=1;
};
message StepSimulationCommand
{
};
message LoadUrdfCommand {
string urdfFileName=1;
vec3 initialPosition=2;
quat4 initialOrientation=3;
//for why oneof here, see the sad decision here:
//https://github.com/protocolbuffers/protobuf/issues/1606
oneof hasUseMultiBody { int32 useMultiBody=4; }
oneof hasUseFixedBase{ bool useFixedBase=5; }
int32 urdfFlags=6;
oneof hasGlobalScaling { double globalScaling=7;
}
};
message LoadUrdfStatus {
int32 objectUniqueId=1;
}
// The request message containing the command
message PyBulletCommand {
int32 commandType=1;
oneof commands {
LoadUrdfCommand loadUrdfCommand = 3;
TerminateServerCommand terminateServerCommand=4;
StepSimulationCommand stepSimulationCommand= 5;
}
}
// The response message containing the status
message PyBulletStatus {
int32 statusType=1;
oneof status
{
LoadUrdfStatus urdfStatus = 2;
}
}
// Generated by the gRPC C++ plugin.
// If you make any local change, they will be lost.
// source: pybullet.proto
#include "pybullet.pb.h"
#include "pybullet.grpc.pb.h"
#include <grpc++/impl/codegen/async_stream.h>
#include <grpc++/impl/codegen/async_unary_call.h>
#include <grpc++/impl/codegen/channel_interface.h>
#include <grpc++/impl/codegen/client_unary_call.h>
#include <grpc++/impl/codegen/method_handler_impl.h>
#include <grpc++/impl/codegen/rpc_service_method.h>
#include <grpc++/impl/codegen/service_type.h>
#include <grpc++/impl/codegen/sync_stream.h>
namespace pybullet_grpc {
static const char* PyBulletAPI_method_names[] = {
"/pybullet_grpc.PyBulletAPI/SubmitCommand",
};
std::unique_ptr< PyBulletAPI::Stub> PyBulletAPI::NewStub(const std::shared_ptr< ::grpc::ChannelInterface>& channel, const ::grpc::StubOptions& options) {
std::unique_ptr< PyBulletAPI::Stub> stub(new PyBulletAPI::Stub(channel));
return stub;
}
PyBulletAPI::Stub::Stub(const std::shared_ptr< ::grpc::ChannelInterface>& channel)
: channel_(channel), rpcmethod_SubmitCommand_(PyBulletAPI_method_names[0], ::grpc::RpcMethod::NORMAL_RPC, channel)
{}
::grpc::Status PyBulletAPI::Stub::SubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::pybullet_grpc::PyBulletStatus* response) {
return ::grpc::BlockingUnaryCall(channel_.get(), rpcmethod_SubmitCommand_, context, request, response);
}
::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>* PyBulletAPI::Stub::AsyncSubmitCommandRaw(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) {
return new ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>(channel_.get(), cq, rpcmethod_SubmitCommand_, context, request);
}
PyBulletAPI::Service::Service() {
AddMethod(new ::grpc::RpcServiceMethod(
PyBulletAPI_method_names[0],
::grpc::RpcMethod::NORMAL_RPC,
new ::grpc::RpcMethodHandler< PyBulletAPI::Service, ::pybullet_grpc::PyBulletCommand, ::pybullet_grpc::PyBulletStatus>(
std::mem_fn(&PyBulletAPI::Service::SubmitCommand), this)));
}
PyBulletAPI::Service::~Service() {
}
::grpc::Status PyBulletAPI::Service::SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) {
(void) context;
(void) request;
(void) response;
return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
}
} // namespace pybullet_grpc
// Generated by the gRPC C++ plugin.
// If you make any local change, they will be lost.
// source: pybullet.proto
#ifndef GRPC_pybullet_2eproto__INCLUDED
#define GRPC_pybullet_2eproto__INCLUDED
#include "pybullet.pb.h"
#include <grpc++/impl/codegen/async_stream.h>
#include <grpc++/impl/codegen/async_unary_call.h>
#include <grpc++/impl/codegen/method_handler_impl.h>
#include <grpc++/impl/codegen/proto_utils.h>
#include <grpc++/impl/codegen/rpc_method.h>
#include <grpc++/impl/codegen/service_type.h>
#include <grpc++/impl/codegen/status.h>
#include <grpc++/impl/codegen/stub_options.h>
#include <grpc++/impl/codegen/sync_stream.h>
namespace grpc {
class CompletionQueue;
class Channel;
class RpcService;
class ServerCompletionQueue;
class ServerContext;
} // namespace grpc
namespace pybullet_grpc {
class PyBulletAPI final {
public:
class StubInterface {
public:
virtual ~StubInterface() {}
// Sends a greeting
virtual ::grpc::Status SubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::pybullet_grpc::PyBulletStatus* response) = 0;
std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::pybullet_grpc::PyBulletStatus>> AsyncSubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) {
return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::pybullet_grpc::PyBulletStatus>>(AsyncSubmitCommandRaw(context, request, cq));
}
private:
virtual ::grpc::ClientAsyncResponseReaderInterface< ::pybullet_grpc::PyBulletStatus>* AsyncSubmitCommandRaw(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) = 0;
};
class Stub final : public StubInterface {
public:
Stub(const std::shared_ptr< ::grpc::ChannelInterface>& channel);
::grpc::Status SubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::pybullet_grpc::PyBulletStatus* response) override;
std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>> AsyncSubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) {
return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>>(AsyncSubmitCommandRaw(context, request, cq));
}
private:
std::shared_ptr< ::grpc::ChannelInterface> channel_;
::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>* AsyncSubmitCommandRaw(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) override;
const ::grpc::RpcMethod rpcmethod_SubmitCommand_;
};
static std::unique_ptr<Stub> NewStub(const std::shared_ptr< ::grpc::ChannelInterface>& channel, const ::grpc::StubOptions& options = ::grpc::StubOptions());
class Service : public ::grpc::Service {
public:
Service();
virtual ~Service();
// Sends a greeting
virtual ::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response);
};
template <class BaseClass>
class WithAsyncMethod_SubmitCommand : public BaseClass {
private:
void BaseClassMustBeDerivedFromService(const Service *service) {}
public:
WithAsyncMethod_SubmitCommand() {
::grpc::Service::MarkMethodAsync(0);
}
~WithAsyncMethod_SubmitCommand() override {
BaseClassMustBeDerivedFromService(this);
}
// disable synchronous version of this method
::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) final override {
abort();
return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
}
void RequestSubmitCommand(::grpc::ServerContext* context, ::pybullet_grpc::PyBulletCommand* request, ::grpc::ServerAsyncResponseWriter< ::pybullet_grpc::PyBulletStatus>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) {
::grpc::Service::RequestAsyncUnary(0, context, request, response, new_call_cq, notification_cq, tag);
}
};
typedef WithAsyncMethod_SubmitCommand<Service > AsyncService;
template <class BaseClass>
class WithGenericMethod_SubmitCommand : public BaseClass {
private:
void BaseClassMustBeDerivedFromService(const Service *service) {}
public:
WithGenericMethod_SubmitCommand() {
::grpc::Service::MarkMethodGeneric(0);
}
~WithGenericMethod_SubmitCommand() override {
BaseClassMustBeDerivedFromService(this);
}
// disable synchronous version of this method
::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) final override {
abort();
return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
}
};
template <class BaseClass>
class WithStreamedUnaryMethod_SubmitCommand : public BaseClass {
private:
void BaseClassMustBeDerivedFromService(const Service *service) {}
public:
WithStreamedUnaryMethod_SubmitCommand() {
::grpc::Service::MarkMethodStreamed(0,
new ::grpc::StreamedUnaryHandler< ::pybullet_grpc::PyBulletCommand, ::pybullet_grpc::PyBulletStatus>(std::bind(&WithStreamedUnaryMethod_SubmitCommand<BaseClass>::StreamedSubmitCommand, this, std::placeholders::_1, std::placeholders::_2)));
}
~WithStreamedUnaryMethod_SubmitCommand() override {
BaseClassMustBeDerivedFromService(this);
}
// disable regular version of this method
::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) final override {
abort();
return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
}
// replace default version of method with streamed unary
virtual ::grpc::Status StreamedSubmitCommand(::grpc::ServerContext* context, ::grpc::ServerUnaryStreamer< ::pybullet_grpc::PyBulletCommand,::pybullet_grpc::PyBulletStatus>* server_unary_streamer) = 0;
};
typedef WithStreamedUnaryMethod_SubmitCommand<Service > StreamedUnaryService;
typedef Service SplitStreamedService;
typedef WithStreamedUnaryMethod_SubmitCommand<Service > StreamedService;
};
} // namespace pybullet_grpc
#endif // GRPC_pybullet_2eproto__INCLUDED
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
"""The Python implementation of the PyBullet GRPC client."""
from __future__ import print_function
import grpc
import pybullet_pb2
import pybullet_pb2_grpc
def run():
channel = grpc.insecure_channel('localhost:50051')
stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(urdfFileName="plane.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=False, useFixedBase=True, globalScaling=2, urdfFlags = 1)))
print("PyBullet client received: " , response.statusType)
print("URDF objectid =", response.urdfStatus.objectUniqueId)
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
print("PyBullet client received: " , response.statusType)
#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
#print("PyBullet client received: " , response.statusType)
if __name__ == '__main__':
run()
This diff is collapsed.
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
import grpc
import pybullet_pb2 as pybullet__pb2
class PyBulletAPIStub(object):
# missing associated documentation comment in .proto file
pass
def __init__(self, channel):
"""Constructor.
Args:
channel: A grpc.Channel.
"""
self.SubmitCommand = channel.unary_unary(
'/pybullet_grpc.PyBulletAPI/SubmitCommand',
request_serializer=pybullet__pb2.PyBulletCommand.SerializeToString,
response_deserializer=pybullet__pb2.PyBulletStatus.FromString,
)
class PyBulletAPIServicer(object):
# missing associated documentation comment in .proto file
pass
def SubmitCommand(self, request, context):
"""Sends a greeting
"""
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
context.set_details('Method not implemented!')
raise NotImplementedError('Method not implemented!')
def add_PyBulletAPIServicer_to_server(servicer, server):
rpc_method_handlers = {
'SubmitCommand': grpc.unary_unary_rpc_method_handler(
servicer.SubmitCommand,
request_deserializer=pybullet__pb2.PyBulletCommand.FromString,
response_serializer=pybullet__pb2.PyBulletStatus.SerializeToString,
),
}
generic_handler = grpc.method_handlers_generic_handler(
'pybullet_grpc.PyBulletAPI', rpc_method_handlers)
server.add_generic_rpc_handlers((generic_handler,))
......@@ -466,6 +466,7 @@ end
include "udp"
include "tcp"
include "plugins/testPlugin"
include "plugins/vrSyncPlugin"
include "plugins/tinyRendererPlugin"
......@@ -473,3 +474,6 @@ include "plugins/tinyRendererPlugin"
include "plugins/pdControlPlugin"
include "plugins/collisionFilterPlugin"
if _OPTIONS["grpc"] then
include "grpc"
end
\ No newline at end of file
......@@ -12,7 +12,7 @@ logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings
def setupWorld():
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
......
......@@ -1469,12 +1469,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int constraintSolverType=-1;
double globalCFM = -1;
int minimumSolverIslandSize = -1;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "physicsClientId", NULL};
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
{
return NULL;
}
......@@ -1495,6 +1497,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
}
if (minimumSolverIslandSize >= 0)
{
b3PhysicsParameterSetMinimumSolverIslandSize(command, minimumSolverIslandSize);
}
if (solverResidualThreshold>=0)
{
b3PhysicsParamSetSolverResidualThreshold(command, solverResidualThreshold);
......
......@@ -17,7 +17,7 @@ subject to the following restrictions:
#include "btDbvtBroadphase.h"
#include "LinearMath/btThreads.h"
btScalar gDbvtMargin = btScalar(0.05);
//
// Profiling
//
......@@ -332,12 +332,9 @@ void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
if(delta[0]<0) velocity[0]=-velocity[0];
if(delta[1]<0) velocity[1]=-velocity[1];
if(delta[2]<0) velocity[2]=-velocity[2];
if (
#ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
#else
m_sets[0].update(proxy->leaf,aabb,velocity)
#endif
if (
m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
)
{
++m_updates_done;
......
......@@ -29,7 +29,8 @@ subject to the following restrictions:
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_MARGIN (btScalar)0.05
//#define DBVT_BP_MARGIN (btScalar)0.05
extern btScalar gDbvtMargin;
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256
......
......@@ -455,11 +455,20 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
btBvhTriangleMeshShape* trimesh = 0;
if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
else
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
{
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
}
else
{
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
}
}
if (trimesh==0)
return;
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;
......
......@@ -33,8 +33,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
(void)simplexSolver;
btVector3 guessVectors[] = {
btVector3(transformB.getOrigin() - transformA.getOrigin()),
btVector3(transformA.getOrigin() - transformB.getOrigin()),
btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
btVector3(0, 0, 1),
btVector3(0, 1, 0),
btVector3(1, 0, 0),
......
......@@ -1406,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
}
......@@ -1656,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
void btMultiBodyC