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

enable pdControlPlugin by default (requires pdControlPlugin.cpp and b3RobotSimulatorClientAPI_NoDirect.cpp)

Browse Source 
add pdControl.py example, make pdControlPlugin functional
reduce memory usage
fix examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf, fixes issue #1704
This commit is contained in:
erwincoumans
2018-06-05 15:59:01 -07:00
parent 49eb83c24e
commit b6f5cb4c34
23 changed files with 286 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
View File

@@ -4,6 +4,7 @@
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -52,31 +53,51 @@ B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext*
{
//apply pd control here, apply forces using the PD gains
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
const MyPDControl& pdControl = obj->m_controllers[i];
//compute torque
//apply torque
int dof1 = 0;
b3JointSensorState actualState;
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
{
if (pdControl.m_maxForce>0)
{
//compute torque
btScalar qActual = actualState.m_jointPosition;
btScalar qdActual = actualState.m_jointVelocity;
btScalar positionError = (pdControl.m_desiredPosition -qActual);
double desiredVelocity = 0;
btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
btClamp(force,-pdControl.m_maxForce,pdControl.m_maxForce);
//apply torque
b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE);
args.m_maxTorqueValue = force;
obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args);
}
}
}
//for each registered pd controller, execute PD control
return 0;
}
B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
obj->m_api.syncBodies();
int numObj = obj->m_api.getNumBodies();
printf("numObj = %d\n", numObj);
//printf("numObj = %d\n", numObj);
//protocol:
//first int is the type of command
@@ -89,20 +110,6 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
switch (arguments->m_ints[0])
{
case eAddPDControl:
{
if (arguments->m_numFloats < 5)
return -1;
MyPDControl controller;
controller.m_desiredPosition = arguments->m_floats[0];
controller.m_desiredVelocity = arguments->m_floats[1];
controller.m_kd = arguments->m_floats[2];
controller.m_kp = arguments->m_floats[3];
controller.m_maxForce = arguments->m_floats[4];
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
obj->m_controllers.push_back(controller);
}
case eSetPDControl:
{
if (arguments->m_numFloats < 5)
@@ -116,14 +123,19 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers[i] = controller;
break;
foundIndex=i;
}
}
if (foundIndex<0)
{
obj->m_controllers.push_back(controller);
}
break;
}
case eRemovePDControl:

6
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
View File

@@ -16,14 +16,12 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
///
enum PDControlCommandEnum
{
eAddPDControl = 1,
eSetPDControl = 2,
eRemovePDControl = 4,
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
#ifdef __cplusplus