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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

278
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp
View File

@@ -9,7 +9,6 @@
#include "Bullet3Common/b3FileUtils.h"
#include "../../Utils/b3ResourcePath.h"
struct MuJoCoPhysicsServerCommandProcessorInternalData
{
bool m_isConnected;
@@ -19,15 +18,15 @@ struct MuJoCoPhysicsServerCommandProcessorInternalData
mjModel* m_mujocoModel;
mjData* m_mujocoData;
b3AlignedObjectArray<int> m_mjcfRecentLoadedBodies;
MuJoCoPhysicsServerCommandProcessorInternalData()
:m_isConnected(false),
m_verboseOutput(false),
m_mujocoModel(0),
m_mujocoData(0),
m_physicsDeltaTime(1./240.),
m_numSimulationSubSteps(0)
: m_isConnected(false),
m_verboseOutput(false),
m_mujocoModel(0),
m_mujocoData(0),
m_physicsDeltaTime(1. / 240.),
m_numSimulationSubSteps(0)
{
}
};
@@ -36,11 +35,10 @@ MuJoCoPhysicsServerCommandProcessor::MuJoCoPhysicsServerCommandProcessor()
{
m_data = new MuJoCoPhysicsServerCommandProcessorInternalData;
}
MuJoCoPhysicsServerCommandProcessor::~MuJoCoPhysicsServerCommandProcessor()
{
delete m_data;
}
bool MuJoCoPhysicsServerCommandProcessor::connect()
@@ -51,13 +49,13 @@ bool MuJoCoPhysicsServerCommandProcessor::connect()
return true;
}
printf("MuJoCo Pro library version %.2lf\n", 0.01*mj_version());
if( mjVERSION_HEADER!=mj_version() )
mju_error("Headers and library have different versions");
printf("MuJoCo Pro library version %.2lf\n", 0.01 * mj_version());
if (mjVERSION_HEADER != mj_version())
mju_error("Headers and library have different versions");
// activate MuJoCo license
int result = mj_activate("mjkey.txt");
if (result==1)
// activate MuJoCo license
int result = mj_activate("mjkey.txt");
if (result == 1)
{
m_data->m_isConnected = true;
return true;
@@ -71,7 +69,7 @@ void MuJoCoPhysicsServerCommandProcessor::resetSimulation()
if (m_data->m_mujocoModel)
{
mj_deleteModel(m_data->m_mujocoModel);
m_data->m_mujocoModel=0;
m_data->m_mujocoModel = 0;
}
if (m_data->m_mujocoData)
{
@@ -83,7 +81,7 @@ void MuJoCoPhysicsServerCommandProcessor::resetSimulation()
void MuJoCoPhysicsServerCommandProcessor::disconnect()
{
resetSimulation();
m_data->m_isConnected = false;
}
@@ -101,7 +99,6 @@ bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemo
bool hasStatus = false;
serverStatusOut.m_type = CMD_INVALID_STATUS;
serverStatusOut.m_numDataStreamBytes = 0;
serverStatusOut.m_dataStream = 0;
@@ -110,61 +107,60 @@ bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemo
switch (clientCmd.m_type)
{
case CMD_REQUEST_INTERNAL_DATA:
{
hasStatus = processRequestInternalDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
};
case CMD_SYNC_BODY_INFO:
{
hasStatus = processSyncBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SYNC_USER_DATA:
{
hasStatus = processSyncUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_MJCF:
{
hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_BODY_INFO:
{
hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
hasStatus = processForwardDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
};
case CMD_SYNC_BODY_INFO:
{
hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SYNC_USER_DATA:
{
hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_LOAD_MJCF:
{
hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_BODY_INFO:
{
hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
hasStatus = processSendPhysicsParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
};
case CMD_REQUEST_ACTUAL_STATE:
{
hasStatus = processRequestActualStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_RESET_SIMULATION:
{
hasStatus = processResetSimulationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
default:
case CMD_RESET_SIMULATION:
{
hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");
printf("Unknown command encountered: %d",clientCmd.m_type);
SharedMemoryStatus& serverCmd =serverStatusOut;
printf("Unknown command encountered: %d", clientCmd.m_type);
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
hasStatus = true;
}
@@ -467,7 +463,6 @@ bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemo
break;
}
#endif
};
return hasStatus;
@@ -483,8 +478,6 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestInternalDataCommand(cons
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -513,37 +506,39 @@ bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct Sh
BT_PROFILE("CMD_LOAD_MJCF");
serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED;
const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments;
if (m_data->m_verboseOutput)
{
printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName);
}
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true;
if (m_data->m_verboseOutput)
{
printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName);
}
bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody != 0) : true;
int flags = 0;
if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
{
flags |= clientCmd.m_mjcfArguments.m_flags;
}
const char* fileName = mjcfArgs.m_mjcfFileName;
if (strlen(fileName)>0)
if (strlen(fileName) > 0)
{
char relativeFileName[1024];
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
if (!fileFound){
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024) > 0);
if (!fileFound)
{
printf("MJCF file not found: %s\n", fileName);
} else
}
else
{
int maxPathLen = 1024;
char pathPrefix[1024];
fu.extractPath(relativeFileName,pathPrefix,maxPathLen);
fu.extractPath(relativeFileName, pathPrefix, maxPathLen);
{
char error[1000] = "could not load binary model";
mjModel* mnew = 0;
if( strlen(relativeFileName)>4 && !strcmp(relativeFileName+strlen(relativeFileName)-4, ".mjb") )
char error[1000] = "could not load binary model";
mjModel* mnew = 0;
if (strlen(relativeFileName) > 4 && !strcmp(relativeFileName + strlen(relativeFileName) - 4, ".mjb"))
{
mnew = mj_loadModel(relativeFileName, 0);
}
@@ -566,34 +561,29 @@ bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct Sh
mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
}
}
if( !mnew )
if (!mnew)
{
printf("%s\n", error);
} else
}
else
{
int maxBodies = btMin(MAX_SDF_BODIES, mnew->nbody);
serverStatusOut.m_sdfLoadedArgs.m_numBodies = maxBodies;
for (int i=0;i<maxBodies;i++)
for (int i = 0; i < maxBodies; i++)
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i]=i;
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = i;
}
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED;
}
}
}
}
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -601,31 +591,29 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestBodyInfoCommand(const st
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
//stream info into memory
int streamSizeInBytes = 0;//createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
int streamSizeInBytes = 0; //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0;
if (m_data->m_mujocoModel && sdfInfoArgs.m_bodyUniqueId>=0 && sdfInfoArgs.m_bodyUniqueId<m_data->m_mujocoModel->nbody)
if (m_data->m_mujocoModel && sdfInfoArgs.m_bodyUniqueId >= 0 && sdfInfoArgs.m_bodyUniqueId < m_data->m_mujocoModel->nbody)
{
const char* name = m_data->m_mujocoModel->names+m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId];
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,name);
const char* name = m_data->m_mujocoModel->names + m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId];
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, name);
}
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
if (m_data->m_mujocoModel)
{
if (m_data->m_verboseOutput)
@@ -633,39 +621,37 @@ bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const st
b3Printf("Step simulation request");
b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime;
if (m_data->m_numSimulationSubSteps > 0)
{
for (int i=0;i<m_data->m_numSimulationSubSteps;i++)
for (int i = 0; i < m_data->m_numSimulationSubSteps; i++)
{
m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps;
mj_step(m_data->m_mujocoModel,m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData);
m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
mj_step(m_data->m_mujocoModel, m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
}
}
else
{
m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime;
mj_step(m_data->m_mujocoModel,m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData);
mj_step(m_data->m_mujocoModel, m_data->m_mujocoData);
mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
{
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
}
@@ -718,7 +704,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(co
this->m_data->m_dynamicsWorld->setGravity(grav);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_dynamicsWorld->getWorldInfo().m_gravity=grav;
#endif
if (m_data->m_verboseOutput)
{
@@ -812,7 +798,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(co
#endif
SharedMemoryStatus& serverCmd =serverStatusOut;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;
}
@@ -821,7 +807,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
{
bool hasStatus = true;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
if (m_data->m_mujocoModel)
{
BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
@@ -831,26 +817,26 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
if (bodyUniqueId>=0 && bodyUniqueId<m_data->m_mujocoModel->nbody)
if (bodyUniqueId >= 0 && bodyUniqueId < m_data->m_mujocoModel->nbody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;//todo body->m_multiBody->getNumLinks();
serverCmd.m_sendActualStateArgs.m_numLinks = 0; //todo body->m_multiBody->getNumLinks();
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
if (serverCmd.m_sendActualStateArgs.m_numLinks>= MAX_DEGREE_OF_FREEDOM)
if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM)
{
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;
return hasStatus;
}
bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0);
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0);
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0);
if (computeForwardKinematics || computeLinkVelocities)
{
@@ -862,18 +848,18 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
int rootLink = 0;//todo check
int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
int rootLink = 0; //todo check
int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
//assume mjJNT_FREE?
int qposAdr = (m_data->m_mujocoModel->jnt_qposadr+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
mjtNum* pos = m_data->m_mujocoData->xipos+bodyUniqueId*3;
int qposAdr = (m_data->m_mujocoModel->jnt_qposadr + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
mjtNum* pos = m_data->m_mujocoData->xipos + bodyUniqueId * 3;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0;
mjtNum* orn= m_data->m_mujocoData->xquat+bodyUniqueId*4;
mjtNum* cvel=m_data->m_mujocoData->cvel+bodyUniqueId*6;
mjtNum* orn = m_data->m_mujocoData->xquat + bodyUniqueId * 4;
mjtNum* cvel = m_data->m_mujocoData->cvel + bodyUniqueId * 6;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0;
@@ -890,31 +876,30 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
totalDegreeOfFreedomQ += 7; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3];//mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4];//mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5];//mb->getBaseVel()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3]; //mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4]; //mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5]; //mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0];//mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1];//mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2];//mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0]; //mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1]; //mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2]; //mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
}
//btAlignedObjectArray<btVector3> omega;
//btAlignedObjectArray<btVector3> linVel;
int numLinks = m_data->m_mujocoModel->body_jntnum[bodyUniqueId];
for (int l=0;l<numLinks ;l++)
for (int l = 0; l < numLinks; l++)
{
int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
//int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
#if 0
#if 0
mjtNum* xpos =
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
@@ -944,7 +929,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
}
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
#if 0
#if 0
if (supportsJointMotor(mb,l))
{
if (motor && m_data->m_physicsDeltaTime>btScalar(0))
@@ -952,7 +937,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
}
}
#endif
#endif
//btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
//btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
@@ -966,9 +951,8 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = 0;//linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = 0;//linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = 1;//linkCOMRotation.w();
#if 0
#if 0
btVector3 worldLinVel(0,0,0);
btVector3 worldAngVel(0,0,0);
@@ -978,7 +962,7 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
worldLinVel = linkRotMat * linVel[l+1];
worldAngVel = linkRotMat * omega[l+1];
}
#endif
#endif
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = 0;//worldLinVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = 0;//worldLinVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = 0;//worldLinVel[2];
@@ -994,39 +978,33 @@ bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = 0;//linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = 0;//linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = 1;//linkLocalInertialRotation.w();
#endif
#endif
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
hasStatus = true;
}
}
}
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_RESET_SIMULATION");
resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
return hasStatus;
}
bool MuJoCoPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
return false;
}
#endif //BT_ENABLE_MUJOCO
#endif //BT_ENABLE_MUJOCO