more work on proto/pybullet.proto
This commit is contained in:
erwincoumans
@@ -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]);
|
||||
|
||||
break;
|
||||
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");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user