mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
433 lines
15 KiB
C++
433 lines
15 KiB
C++
#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"
|
|
#include "LinearMath/btMinMax.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())
|
|
{
|
|
const ::pybullet_grpc::LoadUrdfCommand& grpcCmd = grpcCommand.loadurdfcommand();
|
|
|
|
std::string fileName = grpcCmd.filename();
|
|
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.flags())
|
|
{
|
|
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;
|
|
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_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");
|
|
}
|
|
|
|
}
|
|
|
|
return converted;
|
|
}
|