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

Merge pull request #425 from erwincoumans/master

Browse Source 
implement preliminary CMD_SEND_DESIRED_STATE with CONTROL_MODE_VELOCI…
This commit is contained in:
erwincoumans
2015-07-14 10:09:37 -07:00
parent 70837d2d0b 2e0da2b7a1
commit 6836121e29
4 changed files with 201 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -64,6 +64,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
case CMD_REQUEST_ACTUAL_STATE:
case CMD_STEP_FORWARD_SIMULATION:
case CMD_SHUTDOWN:
case CMD_SEND_DESIRED_STATE:
case CMD_SEND_BULLET_DATA_STREAM:
{
cl->submitCommand(buttonId);
@@ -133,7 +134,7 @@ void PhysicsClient::initPhysics()
{
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_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);
@@ -235,6 +236,10 @@ void PhysicsClient::processServerCommands()
}
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
break;
@@ -335,6 +340,7 @@ void PhysicsClient::createClientCommand()
switch (command)
{
case CMD_LOAD_URDF:
{
if (!m_serverLoadUrdfOK)
@@ -434,6 +440,51 @@ void PhysicsClient::createClientCommand()
}
break;
}
case CMD_SEND_DESIRED_STATE:
{
if (m_serverLoadUrdfOK)
{
b3Printf("Sending desired state (pos, vel, torque)\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE;
//todo: expose a drop box in the GUI for this
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
m_testBlock1->m_desiredStateQdot[i] = 1;
m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
case CONTROL_MODE_TORQUE:
{
m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
default:
{
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
btAssert(0);
}
}
m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("Cannot send CMD_SEND_DESIRED_STATE, no URDF loaded\n");
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
if (m_serverLoadUrdfOK)

133
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -10,6 +10,7 @@
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "SharedMemoryCommon.h"
//const char* blaatnaam = "basename";
@@ -31,9 +32,12 @@ class PhysicsServer : public SharedMemoryCommon
btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
btAlignedObjectArray<std::string*> m_strings;
bool m_wantsShutdown;
void createJointMotors(btMultiBody* body);
bool supportsJointMotor(btMultiBody* body, int linkIndex);
public:
PhysicsServer(GUIHelperInterface* helper);
@@ -110,7 +114,7 @@ void PhysicsServer::initPhysics()
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = -9.8;
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
@@ -143,6 +147,36 @@ bool PhysicsServer::wantsTermination()
return m_wantsShutdown;
}
bool PhysicsServer::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic);
return canHaveMotor;
}
//for testing, create joint motors for revolute and prismatic joints
void PhysicsServer::createJointMotors(btMultiBody* mb)
{
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
if (supportsJointMotor(mb,mbLinkIndex))
{
float maxMotorImpulse = 0.f;
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
m_dynamicsWorld->addMultiBodyConstraint(motor);
}
}
}
bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase)
{
@@ -167,6 +201,9 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
{
if (mb)
{
createJointMotors(mb);
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_urdfLinkNameMapper.push_back(util);
util->m_mb = mb;
@@ -208,7 +245,7 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
return true;
} else
{
b3Warning("No multibody loaded from URDF");
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
return false;
}
} else
@@ -307,6 +344,98 @@ void PhysicsServer::stepSimulation(float deltaTime)
break;
}
case CMD_SEND_DESIRED_STATE:
{
//for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
//{
// m_testBlock1->m_desiredStateForceTorque[i] = 100;
//}
b3Printf("Processed CMD_SEND_DESIRED_STATE");
if (m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0);
btAssert(mb);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{
case CONTROL_MODE_TORQUE:
{
b3Printf("Using CONTROL_MODE_TORQUE");
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(m_testBlock1->m_desiredStateForceTorque[0],
m_testBlock1->m_desiredStateForceTorque[1],
m_testBlock1->m_desiredStateForceTorque[2]);
btVector3 t(m_testBlock1->m_desiredStateForceTorque[3],
m_testBlock1->m_desiredStateForceTorque[4],
m_testBlock1->m_desiredStateForceTorque[5]);
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
for (int link=0;link<mb->getNumLinks();link++)
{
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
double torque = m_testBlock1->m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
}
break;
}
case CONTROL_MODE_VELOCITY:
{
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
if (m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0);
int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor** motorPtr = m_multiBodyJointMotorMap[link];
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = m_testBlock1->m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation'
btScalar maxImp = m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
dofIndex += mb->getLink(link).m_dofCount;
}
}
b3Printf("Using CONTROL_MODE_TORQUE with %d motors", numMotors);
break;
}
default:
{
b3Printf("m_controlMode not implemented yet");
break;
}
}
}
SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0];
serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
m_testBlock1->m_numServerCommands++;
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
b3Printf("Sending the actual state (Q,U)");

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -63,9 +63,17 @@ struct SetJointFeedbackArgs
bool m_isEnabled;
};
//todo: discuss and decide about control mode and combinations
enum {
// POSITION_CONTROL=0,
CONTROL_MODE_VELOCITY,
CONTROL_MODE_TORQUE,
};
struct SendDesiredStateArgs
{
int m_bodyUniqueId;
int m_controlMode;
};
struct RequestActualStateArgs
@@ -109,7 +117,7 @@ struct SharedMemoryCommand
UrdfArgs m_urdfArguments;
BulletDataStreamArgs m_dataStreamArguments;
StepSimulationArgs m_stepSimulationArguments;
SendDesiredStateArgs m_sendDesiredQandUStateCommandArgument;
SendDesiredStateArgs m_sendDesiredStateCommandArgument;
RequestActualStateArgs m_requestActualStateInformationCommandArgument;
SendActualStateArgs m_sendActualStateArgs;
};

9
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -26,13 +26,22 @@ struct SharedMemoryExampleData
//desired state is only written by the client, read-only access by server is expected
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
//or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode
double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
//the Bullet data structures are more general purpose than the capabilities of a URDF file.
char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
//m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for
//example to provide all details of a multibody including joint/link names, after loading a URDF file.
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
};