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

Merge pull request #597 from erwincoumans/master

Browse Source 
add joint damping in PhysicsServerCommandProcessor, add some comments
This commit is contained in:
erwincoumans
2016-04-13 17:35:44 -07:00
parent 5181592459 0ca56e756f
commit 4dfbd2525f
3 changed files with 33 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -74,8 +74,10 @@ b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle ph
b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
///all those commands are optional, except for the *Init
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
@@ -109,16 +111,21 @@ int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandH
int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles.
///This will set all velocities of base and joints to zero.
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
///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.
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);

22
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1415,6 +1415,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
b3Printf("Step simulation request");
}
///todo(erwincoumans) move this damping inside Bullet
for (int i=0;i<m_data->m_bodyHandles.size();i++)
{
applyJointDamping(i);
}
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -1883,3 +1888,20 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
{
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body) {
btMultiBody* mb = body->m_multiBody;
if (mb) {
for (int l=0;l<mb->getNumLinks();l++) {
for (int d=0;d<mb->getLink(l).m_dofCount;d++) {
double damping_coefficient = mb->getLink(l).m_jointDamping;
double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d];
mb->addJointTorqueMultiDof(l, d, damping);
}
}
}
}
}

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -48,7 +48,7 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
void applyJointDamping(int bodyUniqueId);
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H