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

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

@@ -4,7 +4,6 @@
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -28,7 +27,7 @@ struct MyPDControlContainer
btAlignedObjectArray<MyPDControl> m_controllers;
b3RobotSimulatorClientAPI_NoDirect m_api;
MyPDControlContainer()
:m_testData(42)
: m_testData(42)
{
}
virtual ~MyPDControlContainer()
@@ -48,54 +47,50 @@ B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context)
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
{
//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];
int dof1 = 0;
b3JointSensorState actualState;
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex, &actualState))
{
if (pdControl.m_maxForce>0)
if (pdControl.m_maxForce > 0)
{
//compute torque
btScalar qActual = actualState.m_jointPosition;
btScalar qdActual = actualState.m_jointVelocity;
btScalar positionError = (pdControl.m_desiredPosition -qActual);
btScalar positionError = (pdControl.m_desiredPosition - qActual);
double desiredVelocity = 0;
btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
btScalar velocityError = (pdControl.m_desiredVelocity - qdActual);
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd * velocityError;
btClamp(force, -pdControl.m_maxForce, pdControl.m_maxForce);
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);
}
}
}
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);
@@ -109,50 +104,50 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
switch (arguments->m_ints[0])
{
case eSetPDControl:
{
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];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
case eSetPDControl:
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
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];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
obj->m_controllers[i] = controller;
foundIndex=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;
foundIndex = i;
}
}
if (foundIndex < 0)
{
obj->m_controllers.push_back(controller);
}
break;
}
if (foundIndex<0)
case eRemovePDControl:
{
obj->m_controllers.push_back(controller);
}
break;
}
case eRemovePDControl:
{
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
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)
for (int i = 0; i < obj->m_controllers.size(); i++)
{
obj->m_controllers.removeAtIndex(i);
break;
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers.removeAtIndex(i);
break;
}
}
break;
}
break;
}
default:
default:
{
return -1;
}
@@ -162,11 +157,9 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
return result;
}
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer*) context->m_userPointer;
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

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

@@ -4,28 +4,27 @@
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//the following 3 APIs are required
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
///
enum PDControlCommandEnum
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
///
enum PDControlCommandEnum
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define PID_CONTROL_PLUGIN_H
#endif //#define PID_CONTROL_PLUGIN_H