mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
081a40d254
need to add streaming because memory is too small to store all lines initial test of PD control in physics server, need to switch to PD control for motor constraint, instead of using external forces.
376 lines
11 KiB
C++
376 lines
11 KiB
C++
|
|
#include "PhysicsClientExample.h"
|
|
|
|
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
|
|
|
#include "SharedMemoryCommon.h"
|
|
#include "../CommonInterfaces/CommonParameterInterface.h"
|
|
|
|
#include "PhysicsClient.h"
|
|
#include "SharedMemoryCommands.h"
|
|
|
|
struct MyMotorInfo2
|
|
{
|
|
btScalar m_velTarget;
|
|
btScalar m_maxForce;
|
|
int m_uIndex;
|
|
};
|
|
|
|
#define MAX_NUM_MOTORS 128
|
|
|
|
class PhysicsClientExample : public SharedMemoryCommon
|
|
{
|
|
protected:
|
|
PhysicsClientSharedMemory m_physicsClient;
|
|
|
|
|
|
bool m_wantsTermination;
|
|
btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
|
|
|
|
void createButton(const char* name, int id, bool isTrigger );
|
|
|
|
void createButtons();
|
|
|
|
public:
|
|
|
|
//@todo, add accessor methods
|
|
MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
|
|
int m_numMotors;
|
|
|
|
|
|
PhysicsClientExample(GUIHelperInterface* helper);
|
|
virtual ~PhysicsClientExample();
|
|
|
|
virtual void initPhysics();
|
|
|
|
virtual void stepSimulation(float deltaTime);
|
|
|
|
virtual void resetCamera()
|
|
{
|
|
float dist = 5;
|
|
float pitch = 50;
|
|
float yaw = 35;
|
|
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
|
|
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
|
|
|
|
}
|
|
|
|
virtual bool wantsTermination()
|
|
{
|
|
return m_wantsTermination;
|
|
}
|
|
|
|
virtual bool isConnected()
|
|
{
|
|
return m_physicsClient.isConnected();
|
|
}
|
|
|
|
void enqueueCommand(const SharedMemoryCommand& orgCommand);
|
|
|
|
virtual void exitPhysics(){};
|
|
virtual void renderScene()
|
|
{
|
|
int numLines = m_physicsClient.getNumDebugLines();
|
|
const btVector3* fromLines = m_physicsClient.getDebugLinesFrom();
|
|
const btVector3* toLines = m_physicsClient.getDebugLinesTo();
|
|
const btVector3* colorLines = m_physicsClient.getDebugLinesColor();
|
|
btScalar lineWidth = 2;
|
|
for (int i=0;i<numLines;i++)
|
|
{
|
|
this->m_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth);
|
|
}
|
|
}
|
|
virtual void physicsDebugDraw(int debugFlags){}
|
|
virtual bool mouseMoveCallback(float x,float y){return false;};
|
|
virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;}
|
|
virtual bool keyboardCallback(int key, int state){return false;}
|
|
|
|
|
|
virtual void setSharedMemoryKey(int key)
|
|
{
|
|
m_physicsClient.setSharedMemoryKey(key);
|
|
}
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
void MyCallback(int buttonId, bool buttonState, void* userPtr)
|
|
{
|
|
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
|
|
|
|
SharedMemoryCommand command;
|
|
|
|
switch (buttonId)
|
|
{
|
|
case CMD_LOAD_URDF:
|
|
{
|
|
command.m_type =CMD_LOAD_URDF;
|
|
sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");
|
|
command.m_urdfArguments.m_initialPosition[0] = 0.0;
|
|
command.m_updateFlags =
|
|
URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE;
|
|
|
|
command.m_urdfArguments.m_initialPosition[1] = 0.0;
|
|
command.m_urdfArguments.m_initialPosition[2] = 0.0;
|
|
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
|
|
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
|
|
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
|
|
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
|
|
command.m_urdfArguments.m_useFixedBase = false;
|
|
command.m_urdfArguments.m_useMultiBody = true;
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
}
|
|
case CMD_CREATE_BOX_COLLISION_SHAPE:
|
|
{
|
|
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
|
|
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
|
|
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
|
|
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
|
|
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
|
|
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
}
|
|
case CMD_REQUEST_ACTUAL_STATE:
|
|
{
|
|
command.m_type =CMD_REQUEST_ACTUAL_STATE;
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
};
|
|
case CMD_STEP_FORWARD_SIMULATION:
|
|
{
|
|
command.m_type =CMD_STEP_FORWARD_SIMULATION;
|
|
cl->enqueueCommand(command);
|
|
command.m_type =CMD_REQUEST_DEBUG_LINES;
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
}
|
|
|
|
case CMD_SEND_DESIRED_STATE:
|
|
{
|
|
|
|
command.m_type =CMD_SEND_DESIRED_STATE;
|
|
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
|
|
|
|
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
|
|
|
|
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
|
|
{
|
|
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
|
|
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
|
|
}
|
|
for (int i=0;i<cl->m_numMotors;i++)
|
|
{
|
|
btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
|
|
|
|
int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
|
|
if (targetVel>1)
|
|
{
|
|
printf("testme");
|
|
}
|
|
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
|
|
|
|
}
|
|
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
}
|
|
case CMD_RESET_SIMULATION:
|
|
{
|
|
command.m_type = CMD_RESET_SIMULATION;
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
}
|
|
case CMD_SEND_BULLET_DATA_STREAM:
|
|
{
|
|
command.m_type = buttonId;
|
|
cl->enqueueCommand(command);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
b3Error("Unknown buttonId");
|
|
btAssert(0);
|
|
}
|
|
};
|
|
}
|
|
|
|
void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand)
|
|
{
|
|
m_userCommandRequests.push_back(orgCommand);
|
|
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
|
|
|
|
b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
|
|
}
|
|
|
|
|
|
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
|
|
:SharedMemoryCommon(helper),
|
|
m_wantsTermination(false),
|
|
m_numMotors(0)
|
|
{
|
|
b3Printf("Started PhysicsClientExample\n");
|
|
}
|
|
|
|
PhysicsClientExample::~PhysicsClientExample()
|
|
{
|
|
b3Printf("~PhysicsClientExample\n");
|
|
}
|
|
|
|
void PhysicsClientExample::createButton(const char* name, int buttonId, bool isTrigger )
|
|
{
|
|
ButtonParams button(name,buttonId, isTrigger);
|
|
button.m_callback = MyCallback;
|
|
button.m_userPointer = this;
|
|
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
|
|
}
|
|
|
|
void PhysicsClientExample::createButtons()
|
|
{
|
|
bool isTrigger = false;
|
|
|
|
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
|
|
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
|
|
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
|
|
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
|
|
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
|
|
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
|
|
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
|
|
|
|
}
|
|
|
|
void PhysicsClientExample::initPhysics()
|
|
{
|
|
if (m_guiHelper && m_guiHelper->getParameterInterface())
|
|
{
|
|
int upAxis = 2;
|
|
m_guiHelper->setUpAxis(upAxis);
|
|
|
|
createButtons();
|
|
|
|
} else
|
|
{
|
|
/*
|
|
m_userCommandRequests.push_back(CMD_LOAD_URDF);
|
|
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
|
|
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
|
|
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
|
|
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
|
|
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
|
|
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
|
|
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
|
|
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
|
|
m_userCommandRequests.push_back(CMD_SHUTDOWN);
|
|
*/
|
|
|
|
}
|
|
|
|
if (!m_physicsClient.connect())
|
|
{
|
|
b3Warning("Cannot connect to physics client");
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void PhysicsClientExample::stepSimulation(float deltaTime)
|
|
{
|
|
|
|
if (m_physicsClient.isConnected())
|
|
{
|
|
SharedMemoryStatus status;
|
|
bool hasStatus = m_physicsClient.processServerStatus(status);
|
|
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
|
|
{
|
|
for (int i=0;i<m_physicsClient.getNumJoints();i++)
|
|
{
|
|
b3JointInfo info;
|
|
m_physicsClient.getJointInfo(i,info);
|
|
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
|
|
|
}
|
|
if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED)
|
|
{
|
|
//store the debug lines for drawing
|
|
|
|
|
|
}
|
|
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
|
|
{
|
|
for (int i=0;i<m_physicsClient.getNumJoints();i++)
|
|
{
|
|
b3JointInfo info;
|
|
m_physicsClient.getJointInfo(i,info);
|
|
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
|
|
|
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
|
|
{
|
|
if (m_numMotors<MAX_NUM_MOTORS)
|
|
{
|
|
char motorName[1024];
|
|
sprintf(motorName,"%s q'", info.m_jointName);
|
|
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
|
|
motorInfo->m_velTarget = 0.f;
|
|
motorInfo->m_uIndex = info.m_uIndex;
|
|
|
|
SliderParams slider(motorName,&motorInfo->m_velTarget);
|
|
slider.m_minVal=-4;
|
|
slider.m_maxVal=4;
|
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
|
m_numMotors++;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
if (m_physicsClient.canSubmitCommand())
|
|
{
|
|
if (m_userCommandRequests.size())
|
|
{
|
|
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
|
|
SharedMemoryCommand command = m_userCommandRequests[0];
|
|
|
|
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
|
|
for (int i=1;i<m_userCommandRequests.size();i++)
|
|
{
|
|
m_userCommandRequests[i-1] = m_userCommandRequests[i];
|
|
}
|
|
|
|
m_userCommandRequests.pop_back();
|
|
|
|
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
|
|
if (command.m_type==CMD_RESET_SIMULATION)
|
|
{
|
|
m_guiHelper->getParameterInterface()->removeAllParameters();
|
|
m_numMotors=0;
|
|
createButtons();
|
|
}
|
|
|
|
m_physicsClient.submitClientCommand(command);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
extern int gSharedMemoryKey;
|
|
|
|
|
|
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options)
|
|
{
|
|
PhysicsClientExample* example = new PhysicsClientExample(options.m_guiHelper);
|
|
if (gSharedMemoryKey>=0)
|
|
{
|
|
example->setSharedMemoryKey(gSharedMemoryKey);
|
|
}
|
|
return example;
|
|
}
|