Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more work on proto/pybullet.proto

Browse Source
This commit is contained in:
erwincoumans
2018-09-01 13:49:56 -07:00
parent 9a26d4aaaf
commit 23e84ca9b6
11 changed files with 13387 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -17,18 +17,26 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHa
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
return b3LoadSdfCommandInit2((b3SharedMemoryCommandHandle)command, sdfFileName);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* sdfFileName)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_LOAD_SDF;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
strcpy(command->m_sdfArguments.m_sdfFileName, sdfFileName);
}
else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
@@ -202,6 +210,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientH
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
return b3LoadMJCFCommandInit2((b3SharedMemoryCommandHandle)command, fileName);
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_LOAD_MJCF;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
@@ -215,8 +231,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientH
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
@@ -893,10 +908,17 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3Phys
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
return b3RequestActualStateCommandInit2((b3SharedMemoryCommandHandle)command, bodyUniqueId);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_REQUEST_ACTUAL_STATE;
command->m_updateFlags = 0;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity)
@@ -1594,16 +1616,23 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClien
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
return b3CreatePoseCommandInit2((b3SharedMemoryCommandHandle)command, bodyUniqueId);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_updateFlags = 0;
command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
for (int i = 0; i<MAX_DEGREE_OF_FREEDOM; i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
command->m_initPoseArgs.m_hasInitialStateQdot[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
return commandHandle;
}
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
@@ -1693,6 +1722,23 @@ B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHand
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetQ(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* q, const int* hasQ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
for (int i = 0; i<numJointPositions; i++)
{
if ((i)<MAX_DEGREE_OF_FREEDOM)
{
command->m_initPoseArgs.m_initialStateQ[i] = q[i];
command->m_initPoseArgs.m_hasInitialStateQ[i] = hasQ[i];
}
}
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
@@ -1730,6 +1776,24 @@ B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle ph
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* qDots, const int* hasQdots)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
for (int i = 0; i<numJointVelocities; i++)
{
if (i<MAX_DEGREE_OF_FREEDOM)
{
command->m_initPoseArgs.m_initialStateQdot[i] = qDots[i];
command->m_initPoseArgs.m_hasInitialStateQdot[i] = hasQdots[i];
}
}
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1948,6 +2012,52 @@ B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int l
return 0;
}
B3_SHARED_API int b3GetStatusActualState2(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numLinks,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[],
const double* linkLocalInertialFrames[],
const double* jointMotorForces[],
const double* linkStates[],
const double* linkWorldVelocities[] )
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status);
if (status == 0)
return 0;
b3GetStatusActualState(statusHandle, bodyUniqueId, numDegreeOfFreedomQ, numDegreeOfFreedomU,
rootLocalInertialFrame, actualStateQ, actualStateQdot, jointReactionForces);
const SendActualStateArgs &args = status->m_sendActualStateArgs;
if (numLinks)
{
*numLinks = args.m_numLinks;
}
if (linkLocalInertialFrames)
{
*linkLocalInertialFrames = args.m_linkLocalInertialFrames;
}
if (jointMotorForces)
{
*jointMotorForces = args.m_jointMotorForce;
}
if (linkStates)
{
*linkStates = args.m_linkState;
}
if (linkWorldVelocities)
{
*linkWorldVelocities = args.m_linkWorldVelocities;
}
}
B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
@@ -2317,10 +2427,16 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3Physics
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
return b3GetDynamicsInfoCommandInit2((b3SharedMemoryCommandHandle)command, bodyUniqueId, linkIndex);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_GET_DYNAMICS_INFO;
command->m_getDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_getDynamicsInfoArgs.m_linkIndex = linkIndex;
return (b3SharedMemoryCommandHandle) command;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info)
@@ -2346,12 +2462,18 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClie
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
return b3InitChangeDynamicsInfo2((b3SharedMemoryCommandHandle)command);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo2(b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_type = CMD_CHANGE_DYNAMICS_INFO;
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = -1;
command->m_changeDynamicsInfoArgs.m_linkIndex = -2;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
return commandHandle;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)

28
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -85,6 +85,20 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle
const double* actualStateQdot[],
const double* jointReactionForces[]);
B3_SHARED_API int b3GetStatusActualState2(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numLinks,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[],
const double* linkLocalInertialFrames[],
const double* jointMotorForces[],
const double* linkStates[],
const double* linkWorldVelocities[]);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[/*3*/], double aabbMax[/*3*/]);
@@ -125,10 +139,14 @@ B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUni
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo2(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double localInertiaDiagonal[]);
@@ -367,6 +385,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClien
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
@@ -408,6 +427,8 @@ B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemo
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* sdfFileName);
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
@@ -505,6 +526,8 @@ B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle com
///base orientation and joint angles. This will set all velocities of base and joints to zero.
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId);
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);
@@ -513,6 +536,9 @@ B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryComman
B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
B3_SHARED_API int b3CreatePoseCommandSetQ(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* q, const int* hasQ);
B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* qDots, const int* hasQdots);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
@@ -525,6 +551,8 @@ B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryC
B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId);
B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics);

37
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7148,6 +7148,21 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
if (linkIndex == -1)
{
serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass();
if (mb->getBaseCollider())
{
serverCmd.m_dynamicsInfo.m_activationState = mb->getBaseCollider()->getActivationState();
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getBaseCollider()->getContactProcessingThreshold();
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getBaseCollider()->getCcdSweptSphereRadius();
serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getBaseCollider()->getCollisionFlags()&btCollisionObject::CF_HAS_FRICTION_ANCHOR;
}
else
{
serverCmd.m_dynamicsInfo.m_activationState = 0;
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0;
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0;
serverCmd.m_dynamicsInfo.m_frictionAnchor = 0;
}
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getBaseInertia()[0];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getBaseInertia()[2];
@@ -7161,6 +7176,9 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3];
serverCmd.m_dynamicsInfo.m_angularDamping = body->m_multiBody->getAngularDamping();
serverCmd.m_dynamicsInfo.m_linearDamping = body->m_multiBody->getLinearDamping();
serverCmd.m_dynamicsInfo.m_restitution = mb->getBaseCollider()->getRestitution();
serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getBaseCollider()->getRollingFriction();
serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getBaseCollider()->getSpinningFriction();
@@ -7179,6 +7197,22 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
else
{
serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex);
if (mb->getLinkCollider(linkIndex))
{
serverCmd.m_dynamicsInfo.m_activationState = mb->getLinkCollider(linkIndex)->getActivationState();
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getLinkCollider(linkIndex)->getContactProcessingThreshold();
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getLinkCollider(linkIndex)->getCcdSweptSphereRadius();
serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getLinkCollider(linkIndex)->getCollisionFlags()&btCollisionObject::CF_HAS_FRICTION_ANCHOR;
}
else
{
serverCmd.m_dynamicsInfo.m_activationState = 0;
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0;
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0;
serverCmd.m_dynamicsInfo.m_frictionAnchor = 0;
}
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getLinkInertia(linkIndex)[0];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
@@ -7191,6 +7225,9 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[3];
serverCmd.m_dynamicsInfo.m_angularDamping = body->m_multiBody->getAngularDamping();
serverCmd.m_dynamicsInfo.m_linearDamping = body->m_multiBody->getLinearDamping();
if (mb->getLinkCollider(linkIndex))
{
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -334,6 +334,13 @@ struct b3DynamicsInfo
double m_restitution;
double m_contactStiffness;
double m_contactDamping;
int m_activationState;
double m_angularDamping;
double m_linearDamping;
double m_ccdSweptSphereRadius;
double m_contactProcessingThreshold;
int m_frictionAnchor;
};
// copied from btMultiBodyLink.h

352
examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
View File

@@ -8,6 +8,7 @@
#include <grpc++/grpc++.h>
#include <grpc/support/log.h>
#include "pybullet.grpc.pb.h"
#include "LinearMath/btMinMax.h"
using grpc::Server;
using grpc::ServerAsyncResponseWriter;
@@ -26,9 +27,9 @@ SharedMemoryCommand* convertGRPCAndSubmitCommand(PyBulletCommand& grpcCommand, S
if (grpcCommand.has_loadurdfcommand())
{
auto grpcCmd = grpcCommand.loadurdfcommand();
const ::pybullet_grpc::LoadUrdfCommand& grpcCmd = grpcCommand.loadurdfcommand();
std::string fileName = grpcCmd.urdffilename();
std::string fileName = grpcCmd.filename();
if (fileName.length())
{
cmdPtr = &cmd;
@@ -45,9 +46,9 @@ SharedMemoryCommand* convertGRPCAndSubmitCommand(PyBulletCommand& grpcCommand, S
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)
if (grpcCmd.hasUseMultiBody_case() == ::pybullet_grpc::LoadUrdfCommand::HasUseMultiBodyCase::kUseMultiBody)
{
b3LoadUrdfCommandSetUseMultiBody( commandHandle, grpcCmd.usemultibody());
b3LoadUrdfCommandSetUseMultiBody(commandHandle, grpcCmd.usemultibody());
}
if (grpcCmd.hasGlobalScaling_case() == ::pybullet_grpc::LoadUrdfCommand::HasGlobalScalingCase::kGlobalScaling)
{
@@ -57,14 +58,200 @@ SharedMemoryCommand* convertGRPCAndSubmitCommand(PyBulletCommand& grpcCommand, S
{
b3LoadUrdfCommandSetUseFixedBase(commandHandle, grpcCmd.usefixedbase());
}
if (grpcCmd.urdfflags())
if (grpcCmd.flags())
{
b3LoadUrdfCommandSetFlags(commandHandle, grpcCmd.urdfflags());
b3LoadUrdfCommandSetFlags(commandHandle, grpcCmd.flags());
}
}
}
if (grpcCommand.has_loadsdfcommand())
{
const ::pybullet_grpc::LoadSdfCommand& grpcCmd = grpcCommand.loadsdfcommand();
std::string fileName = grpcCmd.filename();
if (fileName.length())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
b3LoadSdfCommandInit2(commandHandle, fileName.c_str());
if (grpcCmd.hasUseMultiBody_case() == ::pybullet_grpc::LoadSdfCommand::HasUseMultiBodyCase::kUseMultiBody)
{
b3LoadSdfCommandSetUseMultiBody(commandHandle, grpcCmd.usemultibody());
}
if (grpcCmd.hasGlobalScaling_case() == ::pybullet_grpc::LoadSdfCommand::HasGlobalScalingCase::kGlobalScaling)
{
b3LoadSdfCommandSetUseGlobalScaling(commandHandle, grpcCmd.globalscaling());
}
}
}
if (grpcCommand.has_loadmjcfcommand())
{
const pybullet_grpc::LoadMjcfCommand& grpcCmd = grpcCommand.loadmjcfcommand();
std::string fileName = grpcCmd.filename();
if (fileName.length())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
b3LoadMJCFCommandInit2(commandHandle, fileName.c_str());
if (grpcCmd.flags())
{
b3LoadMJCFCommandSetFlags(commandHandle, grpcCmd.flags());
}
}
}
if (grpcCommand.has_changedynamicscommand())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
const ::pybullet_grpc::ChangeDynamicsCommand& grpcCmd = grpcCommand.changedynamicscommand();
int bodyUniqueId = grpcCmd.bodyuniqueid();
int linkIndex = grpcCmd.linkindex();
b3InitChangeDynamicsInfo2(commandHandle);
if (grpcCmd.hasMass_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasMassCase::kMass)
{
b3ChangeDynamicsInfoSetMass(commandHandle, bodyUniqueId, linkIndex, grpcCmd.mass());
}
if (grpcCmd.hasLateralFriction_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasLateralFrictionCase::kLateralFriction)
{
b3ChangeDynamicsInfoSetLateralFriction(commandHandle, bodyUniqueId, linkIndex, grpcCmd.lateralfriction());
}
if (grpcCmd.hasSpinningFriction_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasSpinningFrictionCase::kSpinningFriction)
{
b3ChangeDynamicsInfoSetSpinningFriction(commandHandle, bodyUniqueId, linkIndex, grpcCmd.spinningfriction());
}
if (grpcCmd.hasRollingFriction_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasRollingFrictionCase::kRollingFriction)
{
b3ChangeDynamicsInfoSetRollingFriction(commandHandle, bodyUniqueId, linkIndex, grpcCmd.rollingfriction());
}
if (grpcCmd.hasRestitution_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasRestitutionCase::kRestitution)
{
b3ChangeDynamicsInfoSetRestitution(commandHandle, bodyUniqueId, linkIndex, grpcCmd.restitution());
}
if (grpcCmd.haslinearDamping_case() == ::pybullet_grpc::ChangeDynamicsCommand::HaslinearDampingCase::kLinearDamping)
{
b3ChangeDynamicsInfoSetLinearDamping(commandHandle, bodyUniqueId, grpcCmd.lineardamping());
}
if (grpcCmd.hasangularDamping_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasangularDampingCase::kAngularDamping)
{
b3ChangeDynamicsInfoSetAngularDamping(commandHandle, bodyUniqueId, grpcCmd.angulardamping());
}
bool hasContactDamping = grpcCmd.hasContactDamping_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasContactDampingCase::kContactDamping;
bool hasContactStiffness = grpcCmd.hasContactStiffness_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasContactStiffnessCase::kContactStiffness;
if (hasContactDamping && hasContactStiffness)
{
b3ChangeDynamicsInfoSetContactStiffnessAndDamping(commandHandle, bodyUniqueId, linkIndex, grpcCmd.contactstiffness(), grpcCmd.contactdamping());
}
if (grpcCmd.hasLocalInertiaDiagonal_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasLocalInertiaDiagonalCase::kLocalInertiaDiagonal)
{
double localInertiaDiag[3] = { grpcCmd.localinertiadiagonal().x(), grpcCmd.localinertiadiagonal().y(), grpcCmd.localinertiadiagonal().z() };
b3ChangeDynamicsInfoSetLocalInertiaDiagonal(commandHandle, bodyUniqueId, linkIndex, localInertiaDiag);
}
if (grpcCmd.hasFrictionAnchor_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasFrictionAnchorCase::kFrictionAnchor)
{
b3ChangeDynamicsInfoSetFrictionAnchor(commandHandle, bodyUniqueId, linkIndex, grpcCmd.frictionanchor());
}
if (grpcCmd.hasccdSweptSphereRadius_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasccdSweptSphereRadiusCase::kCcdSweptSphereRadius)
{
b3ChangeDynamicsInfoSetCcdSweptSphereRadius(commandHandle, bodyUniqueId, linkIndex, grpcCmd.ccdsweptsphereradius());
}
if (grpcCmd.hasContactProcessingThreshold_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasContactProcessingThresholdCase::kContactProcessingThreshold)
{
b3ChangeDynamicsInfoSetContactProcessingThreshold(commandHandle, bodyUniqueId, linkIndex, grpcCmd.contactprocessingthreshold());
}
if (grpcCmd.hasActivationState_case() == ::pybullet_grpc::ChangeDynamicsCommand::HasActivationStateCase::kActivationState)
{
b3ChangeDynamicsInfoSetActivationState(commandHandle, bodyUniqueId, grpcCmd.activationstate());
}
}
if (grpcCommand.has_getdynamicscommand())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
const ::pybullet_grpc::GetDynamicsCommand& grpcCmd = grpcCommand.getdynamicscommand();
b3GetDynamicsInfoCommandInit2(commandHandle, grpcCmd.bodyuniqueid(), grpcCmd.linkindex());
}
if (grpcCommand.has_initposecommand())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
const ::pybullet_grpc::InitPoseCommand& grpcCmd = grpcCommand.initposecommand();
b3CreatePoseCommandInit2(commandHandle, grpcCmd.bodyuniqueid());
double initialQ[MAX_DEGREE_OF_FREEDOM] = { 0 };
double initialQdot[MAX_DEGREE_OF_FREEDOM] = { 0 };
int hasInitialQ[MAX_DEGREE_OF_FREEDOM] = { 0 };
int hasInitialQdot[MAX_DEGREE_OF_FREEDOM] = { 0 };
if (grpcCmd.initialstateq_size() == grpcCmd.hasinitialstateq_size())
{
int numInitial = btMin(grpcCmd.initialstateq_size(), MAX_DEGREE_OF_FREEDOM);
for (int i = 0; i < numInitial; i++)
{
initialQ[i] = grpcCmd.initialstateq(i);
hasInitialQ[i] = grpcCmd.hasinitialstateq(i);
}
if (numInitial)
{
b3CreatePoseCommandSetQ(commandHandle, numInitial, initialQ, hasInitialQ);
}
}
else
{
printf("Error: if (grpcCmd.initialstateq_size() != grpcCmd.hasinitialstateq_size())\n");
}
if (grpcCmd.initialstateqdot_size() == grpcCmd.hasinitialstateqdot_size())
{
int numInitial = btMin(grpcCmd.initialstateqdot_size(), MAX_DEGREE_OF_FREEDOM);
for (int i = 0; i < numInitial; i++)
{
initialQdot[i] = grpcCmd.initialstateqdot(i);
hasInitialQdot[i] = grpcCmd.hasinitialstateqdot(i);
}
if (numInitial)
{
b3CreatePoseCommandSetQdots(commandHandle, numInitial, initialQdot, hasInitialQdot);
}
}
else
{
printf("Error: (grpcCmd.initialstateqdot_size() != grpcCmd.hasinitialstateqdot_size())\n");
}
}
if (grpcCommand.has_requestactualstatecommand())
{
cmdPtr = &cmd;
b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
const ::pybullet_grpc::RequestActualStateCommand& grpcCmd = grpcCommand.requestactualstatecommand();
b3RequestActualStateCommandInit2(commandHandle, grpcCmd.bodyuniqueid());
if (grpcCmd.computeforwardkinematics())
{
b3RequestActualStateCommandComputeForwardKinematics(commandHandle, grpcCmd.computeforwardkinematics());
}
if (grpcCmd.computelinkvelocities())
{
b3RequestActualStateCommandComputeLinkVelocity(commandHandle, grpcCmd.computelinkvelocities());
}
}
if (grpcCommand.has_stepsimulationcommand())
{
cmdPtr = &cmd;
@@ -87,10 +274,157 @@ bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferSer
::pybullet_grpc::LoadUrdfStatus* stat = grpcReply.mutable_urdfstatus();
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
int objectUniqueId = b3GetStatusBodyIndex(statusHandle);
stat->set_objectuniqueid(objectUniqueId);
}
stat->set_bodyuniqueid(objectUniqueId);
break;
}
case CMD_SDF_LOADING_COMPLETED:
{
int bodyIndicesOut[MAX_SDF_BODIES];
::pybullet_grpc::SdfLoadedStatus* stat = grpcReply.mutable_sdfstatus();
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
int numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
if (numBodies > MAX_SDF_BODIES)
{
printf("SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES);
}
for (int i = 0; i < numBodies; i++)
{
stat->add_bodyuniqueids(bodyIndicesOut[i]);
}
break;
}
case CMD_MJCF_LOADING_COMPLETED:
{
int bodyIndicesOut[MAX_SDF_BODIES];
::pybullet_grpc::MjcfLoadedStatus* stat = grpcReply.mutable_mjcfstatus();
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
int numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
if (numBodies > MAX_SDF_BODIES)
{
printf("MJCF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES);
}
for (int i = 0; i < numBodies; i++)
{
stat->add_bodyuniqueids(bodyIndicesOut[i]);
}
break;
}
case CMD_GET_DYNAMICS_INFO_COMPLETED:
{
b3DynamicsInfo info;
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
if (b3GetDynamicsInfo(statusHandle, &info))
{
::pybullet_grpc::GetDynamicsStatus*stat = grpcReply.mutable_getdynamicsstatus();
stat->set_mass(info.m_mass);
stat->set_lateralfriction(info.m_lateralFrictionCoeff);
stat->set_spinningfriction(info.m_spinningFrictionCoeff);
stat->set_rollingfriction(info.m_rollingFrictionCoeff);
stat->set_restitution(info.m_restitution);
stat->set_lineardamping(info.m_linearDamping);
stat->set_angulardamping(info.m_angularDamping);
stat->set_contactstiffness(info.m_contactStiffness);
stat->set_contactdamping(info.m_contactDamping);
pybullet_grpc::vec3* localInertia = stat->mutable_localinertiadiagonal();
localInertia->set_x(info.m_localInertialDiagonal[0]);
localInertia->set_y(info.m_localInertialDiagonal[1]);
localInertia->set_z(info.m_localInertialDiagonal[2]);
stat->set_frictionanchor(info.m_frictionAnchor);
stat->set_ccdsweptsphereradius(info.m_ccdSweptSphereRadius);
stat->set_contactprocessingthreshold(info.m_contactProcessingThreshold);
stat->set_activationstate(info.m_activationState);
}
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
int bodyUniqueId;
int numLinks;
int numDegreeOfFreedomQ;
int numDegreeOfFreedomU;
const double* rootLocalInertialFramePtr=0;
const double* actualStateQptr=0;
const double* actualStateQdotPtr=0;
const double* jointReactionForcesPtr = 0;
const double* linkLocalInertialFrames = 0;
const double* jointMotorForces = 0;
const double* linkStates = 0;
const double* linkWorldVelocities = 0;
if (b3GetStatusActualState2(
statusHandle, &bodyUniqueId,&numLinks,
&numDegreeOfFreedomQ,
&numDegreeOfFreedomU,
&rootLocalInertialFramePtr,
&actualStateQptr,
&actualStateQdotPtr,
&jointReactionForcesPtr,
&linkLocalInertialFrames,
&jointMotorForces,
&linkStates,
&linkWorldVelocities))
{
::pybullet_grpc::SendActualStateStatus* stat = grpcReply.mutable_actualstatestatus();
stat->set_bodyuniqueid(bodyUniqueId);
stat->set_numlinks(numLinks);
stat->set_numdegreeoffreedomq(numDegreeOfFreedomQ);
stat->set_numdegreeoffreedomu(numDegreeOfFreedomU);
for (int i = 0; i < numDegreeOfFreedomQ; i++)
{
stat->add_actualstateq( actualStateQptr[i]);
}
for (int i = 0; i < numDegreeOfFreedomU; i++)
{
stat->add_actualstateqdot( actualStateQdotPtr[i]);
}
for (int i = 0; i < 7; i++)
{
stat->add_rootlocalinertialframe(rootLocalInertialFramePtr[i]);
}
for (int i = 0; i < numLinks * 6; i++)
{
stat->add_linklocalinertialframes( linkLocalInertialFrames[i]);
}
for (int i = 0; i < numLinks * 6; i++)
{
stat->add_jointreactionforces(jointReactionForcesPtr[i]);
}
for (int i = 0; i < numLinks; i++)
{
stat->add_jointmotorforce(jointMotorForces[i]);
}
for (int i = 0; i < numLinks * 7; i++)
{
stat->add_linkstate(linkStates[i]);
}
for (int i = 0; i < numLinks * 6; i++)
{
stat->add_linkworldvelocities(linkWorldVelocities[i]);
}
}
break;
}
case CMD_CLIENT_COMMAND_COMPLETED:
{
//no action needed?
break;
}
default:
{
printf("convertStatusToGRPC: unknown status");
}
}

20
examples/SharedMemory/grpc/premake4.lua
View File

@@ -10,8 +10,10 @@ project ("App_PhysicsServerSharedMemoryBridgeGRPC")
if os.is("Windows") then
defines { "WIN32", "_WIN32_WINNT=0x0600" }
includedirs {"../../ThirdPartyLibs/grpc/include"}
libdirs {"../../ThirdPartyLibs/grpc/lib/win64"}
links {"grpc","grpc++","grpc++_reflection","gpr",
"libprotobuf","crypto","ssl","zlibstaticd","Ws2_32","Winmm" }
"libprotobufd","crypto","ssl","zlibstaticd","Ws2_32","Winmm" }
end
if os.is("Linux") then
defines {"_LINUX"}
@@ -28,6 +30,12 @@ project ("App_PhysicsServerSharedMemoryBridgeGRPC")
files {
"main.cpp",
"ConvertGRPCBullet.cpp",
"ConvertGRPCBullet.h",
"pybullet.grpc.pb.cpp",
"pybullet.grpc.pb.h",
"pybullet.pb.cpp",
"pybullet.pb.h",
"../PhysicsClient.cpp",
"../PhysicsClient.h",
"../PhysicsDirect.cpp",
@@ -68,8 +76,10 @@ links {
if os.is("Windows") then
defines { "WIN32", "_WIN32_WINNT=0x0600" }
includedirs {"../../ThirdPartyLibs/grpc/include"}
libdirs {"../../ThirdPartyLibs/grpc/lib/win64"}
links {"grpc","grpc++","grpc++_reflection","gpr",
"libprotobuf","crypto","ssl","zlibstaticd","Ws2_32","Winmm" }
"libprotobufd","crypto","ssl","zlibstaticd","Ws2_32","Winmm" }
end
if os.is("Linux") then
defines {"_LINUX"}
@@ -146,5 +156,11 @@ myfiles =
files {
myfiles,
"main.cpp",
"ConvertGRPCBullet.cpp",
"ConvertGRPCBullet.h",
"pybullet.grpc.pb.cpp",
"pybullet.grpc.pb.h",
"pybullet.pb.cpp",
"pybullet.pb.h",
}

132
examples/SharedMemory/grpc/proto/pybullet.proto
View File

@@ -1,5 +1,8 @@
syntax = "proto3";
//for why oneof everywhere, see the sad decision here:
//https://github.com/protocolbuffers/protobuf/issues/1606
option java_multiple_files = true;
option java_package = "io.grpc.pybullet_grpc";
option java_outer_classname = "PyBulletProto";
@@ -40,24 +43,134 @@ message StepSimulationCommand
};
message LoadUrdfCommand {
string urdfFileName=1;
string fileName=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;
int32 flags=6;
oneof hasGlobalScaling { double globalScaling=7;
}
};
message LoadUrdfStatus {
int32 objectUniqueId=1;
int32 bodyUniqueId=1;
}
message LoadSdfCommand {
string fileName=1;
oneof hasUseMultiBody { int32 useMultiBody=2; }
oneof hasGlobalScaling { double globalScaling=3;
}
};
message SdfLoadedStatus
{
repeated int32 bodyUniqueIds=2;
}
message LoadMjcfCommand {
string fileName=1;
int32 flags=2;
};
message MjcfLoadedStatus
{
repeated int32 bodyUniqueIds=2;
}
message ChangeDynamicsCommand
{
int32 bodyUniqueId=1;
int32 linkIndex=2;
oneof hasMass { double mass=3;}
oneof hasLateralFriction { double lateralFriction=5;}
oneof hasSpinningFriction {double spinningFriction=6;}
oneof hasRollingFriction {double rollingFriction=7;}
oneof hasRestitution { double restitution=8;}
oneof haslinearDamping { double linearDamping=9;}
oneof hasangularDamping { double angularDamping=10;}
oneof hasContactStiffness { double contactStiffness=11;}
oneof hasContactDamping { double contactDamping=12;}
oneof hasLocalInertiaDiagonal { vec3 localInertiaDiagonal=13;}
oneof hasFrictionAnchor { int32 frictionAnchor=14;}
oneof hasccdSweptSphereRadius { double ccdSweptSphereRadius=15;}
oneof hasContactProcessingThreshold { double contactProcessingThreshold=16;}
oneof hasActivationState { int32 activationState=17;}
};
message GetDynamicsCommand
{
int32 bodyUniqueId=1;
int32 linkIndex=2;
};
message GetDynamicsStatus
{
double mass=3;
double lateralFriction=5;
double spinningFriction=6;
double rollingFriction=7;
double restitution=8;
double linearDamping=9;
double angularDamping=10;
double contactStiffness=11;
double contactDamping=12;
vec3 localInertiaDiagonal=13;
int32 frictionAnchor=14;
double ccdSweptSphereRadius=15;
double contactProcessingThreshold=16;
int32 activationState=17;
};
message InitPoseCommand
{
int32 bodyUniqueId=1;
repeated int32 hasInitialStateQ=2;
repeated double initialStateQ=3;
repeated int32 hasInitialStateQdot=4;
repeated double initialStateQdot=5;
};
message RequestActualStateCommand
{
int32 bodyUniqueId=1;
bool computeForwardKinematics=2;
bool computeLinkVelocities=3;
};
message SendActualStateStatus
{
int32 bodyUniqueId=1;
int32 numLinks=2;
int32 numDegreeOfFreedomQ=3;
int32 numDegreeOfFreedomU=4;
repeated double rootLocalInertialFrame=5;
//actual state is only written by the server, read-only access by client is expected
repeated double actualStateQ=6;
repeated double actualStateQdot=7;
//measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z]
repeated double jointReactionForces=8;
repeated double jointMotorForce=9;
repeated double linkState=10;
repeated double linkWorldVelocities=11;//linear velocity and angular velocity in world space (x/y/z each).
repeated double linkLocalInertialFrames=12;
};
// The request message containing the command
message PyBulletCommand {
@@ -67,9 +180,14 @@ message PyBulletCommand {
LoadUrdfCommand loadUrdfCommand = 3;
TerminateServerCommand terminateServerCommand=4;
StepSimulationCommand stepSimulationCommand= 5;
LoadSdfCommand loadSdfCommand=6;
LoadMjcfCommand loadMjcfCommand=7;
ChangeDynamicsCommand changeDynamicsCommand=8;
GetDynamicsCommand getDynamicsCommand=9;
InitPoseCommand initPoseCommand=10;
RequestActualStateCommand requestActualStateCommand=11;
}
}
// The response message containing the status
@@ -79,6 +197,10 @@ message PyBulletStatus {
oneof status
{
LoadUrdfStatus urdfStatus = 2;
SdfLoadedStatus sdfStatus = 3;
MjcfLoadedStatus mjcfStatus = 4;
GetDynamicsStatus getDynamicsStatus = 5;
SendActualStateStatus actualStateStatus = 6;
}
}

7941
examples/SharedMemory/grpc/pybullet.pb.cpp
View File

File diff suppressed because it is too large Load Diff

3950
examples/SharedMemory/grpc/pybullet.pb.h
View File

File diff suppressed because it is too large Load Diff

46
examples/SharedMemory/grpc/pybullet_client.py
View File

@@ -7,19 +7,55 @@ import grpc
import pybullet_pb2
import pybullet_pb2_grpc
#todo: how to add this?
MJCF_COLORS_FROM_FILE = 512
def run():
print("grpc.insecure_channel")
channel = grpc.insecure_channel('localhost:50051')
print("pybullet_pb2_grpc.PyBulletAPIStub")
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=0
print("submit LoadSdfCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2)))
print("PyBullet client received: " , response)
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
print("PyBullet client received: " , response.statusType)
print("submit LoadMjcfCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadMjcfCommand=pybullet_pb2.LoadMjcfCommand(fileName="mjcf/humanoid.xml",flags=MJCF_COLORS_FROM_FILE)))
print("PyBullet client received: " , response)
print("submit LoadUrdfCommand ")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(fileName="door.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=True, useFixedBase=True, globalScaling=2, flags = 1)))
print("PyBullet client received: " , response)
bodyUniqueId = response.urdfStatus.bodyUniqueId
print("submit ChangeDynamicsCommand ")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10)))
print("PyBullet client received: " , response)
print("submit GetDynamicsCommand ")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(getDynamicsCommand=pybullet_pb2.GetDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1)))
print("PyBullet client received: " , response)
print("submit InitPoseCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(initPoseCommand=pybullet_pb2.InitPoseCommand(bodyUniqueId=bodyUniqueId, initialStateQ=[1,2,3],hasInitialStateQ=[1,1,1])))
print("PyBullet client received: " , response)
print("submit RequestActualStateCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(requestActualStateCommand=pybullet_pb2.RequestActualStateCommand(bodyUniqueId=bodyUniqueId, computeForwardKinematics=True, computeLinkVelocities=True )))
print("PyBullet client received: " , response)
#for i in range (1000):
# print("submit StepSimulationCommand: ", i)
# response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
# print("PyBullet client received: " , response.statusType)
#print("TerminateServerCommand")
#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
#print("PyBullet client received: " , response.statusType)

967
examples/SharedMemory/grpc/pybullet_pb2.py
View File

File diff suppressed because one or more lines are too long