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

Merge pull request #411 from erwincoumans/master

Browse Source 
more work on btMultiBody joint force/torque feedback, velocity target control for rigid body urdf importer
This commit is contained in:
erwincoumans
2015-06-26 13:00:38 -07:00
parent 0a086c8492 bc8b95dd9d
commit 3da6763371
19 changed files with 877 additions and 323 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
data/sphere2.urdf
View File

@@ -43,5 +43,31 @@
<child link="childA"/> <child link="childA"/>
<origin xyz="0 0 1.0"/> <origin xyz="0 0 1.0"/>
</joint> </joint>
<link name="childB">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
<joint name="joint_childA_childB" type="continuous">
<parent link="childA"/>
<child link="childB"/>
<axis xyz="0 1 0"/>
<origin xyz="0 0 0.5"/>
</joint>
</robot> </robot>

162
examples/Constraints/TestHingeTorque.cpp
View File

@@ -5,8 +5,8 @@
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter); short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter);
short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::StaticFilter|btBroadphaseProxy::CharacterFilter)); short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::CharacterFilter));
static btScalar radius(0.2);
struct TestHingeTorque : public CommonRigidBodyBase struct TestHingeTorque : public CommonRigidBodyBase
{ {
@@ -50,7 +50,7 @@ TestHingeTorque::~ TestHingeTorque()
void TestHingeTorque::stepSimulation(float deltaTime) void TestHingeTorque::stepSimulation(float deltaTime)
{ {
if (m_once) if (0)//m_once)
{ {
m_once=false; m_once=false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0); btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
@@ -63,37 +63,41 @@ void TestHingeTorque::stepSimulation(float deltaTime)
} }
m_dynamicsWorld->stepSimulation(1./60,0); m_dynamicsWorld->stepSimulation(1./240,0);
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0], static int count = 0;
base->getAngularVelocity()[1], if ((count& 0x0f)==0)
base->getAngularVelocity()[2]);
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
child->getAngularVelocity()[1],
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
{ {
b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n", btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
m_jointFeedback[i]->m_appliedForceBodyA.x(),
m_jointFeedback[i]->m_appliedForceBodyA.y(), b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
m_jointFeedback[i]->m_appliedForceBodyA.z(), base->getAngularVelocity()[1],
m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
m_jointFeedback[i]->m_appliedTorqueBodyA.y(), base->getAngularVelocity()[2]);
m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
m_jointFeedback[i]->m_appliedForceBodyB.x(), btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(), b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
m_jointFeedback[i]->m_appliedTorqueBodyB.y(), child->getAngularVelocity()[1],
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
{
b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i,
m_jointFeedback[i]->m_appliedForceBodyB.x(),
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
}
} }
count++;
//CommonRigidBodyBase::stepSimulation(deltaTime); //CommonRigidBodyBase::stepSimulation(deltaTime);
} }
@@ -101,10 +105,13 @@ void TestHingeTorque::stepSimulation(float deltaTime)
void TestHingeTorque::initPhysics() void TestHingeTorque::initPhysics()
{ {
m_guiHelper->setUpAxis(1); int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
// m_dynamicsWorld->setGravity(btVector3(0,0,0)); m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe int mode = btIDebugDraw::DBG_DrawWireframe
@@ -115,7 +122,7 @@ void TestHingeTorque::initPhysics()
{ // create a door using hinge constraint attached to the world { // create a door using hinge constraint attached to the world
int numLinks = 1; int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false; bool canSleep = false;
bool selfCollide = false; bool selfCollide = false;
@@ -138,7 +145,9 @@ void TestHingeTorque::initPhysics()
m_dynamicsWorld->removeRigidBody(base); m_dynamicsWorld->removeRigidBody(base);
base->setDamping(0,0); base->setDamping(0,0);
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask); m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
btBoxShape* linkBox = new btBoxShape(linkHalfExtents); btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
btSphereShape* linkSphere = new btSphereShape(radius);
btRigidBody* prevBody = base; btRigidBody* prevBody = base;
for (int i=0;i<numLinks;i++) for (int i=0;i<numLinks;i++)
@@ -148,30 +157,83 @@ void TestHingeTorque::initPhysics()
linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0)); linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox); btCollisionShape* colOb = 0;
if (i==0)
{
colOb = linkBox1;
} else
{
colOb = linkSphere;
}
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb);
m_dynamicsWorld->removeRigidBody(linkBody); m_dynamicsWorld->removeRigidBody(linkBody);
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask); m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
linkBody->setDamping(0,0); linkBody->setDamping(0,0);
//create a hinge constraint btTypedConstraint* con = 0;
btVector3 pivotInA(0,-linkHalfExtents[1],0);
btVector3 pivotInB(0,linkHalfExtents[1],0);
btVector3 axisInA(1,0,0);
btVector3 axisInB(1,0,0);
bool useReferenceA = true;
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
pivotInA,pivotInB,
axisInA,axisInB,useReferenceA);
btJointFeedback* fb = new btJointFeedback();
m_jointFeedback.push_back(fb);
hinge->setJointFeedback(fb);
m_dynamicsWorld->addConstraint(hinge,true); if (i==0)
prevBody = linkBody; {
//create a hinge constraint
btVector3 pivotInA(0,-linkHalfExtents[1],0);
btVector3 pivotInB(0,linkHalfExtents[1],0);
btVector3 axisInA(1,0,0);
btVector3 axisInB(1,0,0);
bool useReferenceA = true;
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
pivotInA,pivotInB,
axisInA,axisInB,useReferenceA);
con = hinge;
} else
{
btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset
btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset
btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody,
pivotInA,pivotInB);
fixed->setLinearLowerLimit(btVector3(0,0,0));
fixed->setLinearUpperLimit(btVector3(0,0,0));
fixed->setAngularLowerLimit(btVector3(0,0,0));
fixed->setAngularUpperLimit(btVector3(0,0,0));
con = fixed;
}
btAssert(con);
if (con)
{
btJointFeedback* fb = new btJointFeedback();
m_jointFeedback.push_back(fb);
con->setJointFeedback(fb);
m_dynamicsWorld->addConstraint(con,true);
}
prevBody = linkBody;
} }
} }
if (1)
{
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
} }

5
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

@@ -129,7 +129,6 @@ void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue)
void GwenParameterInterface::registerButtonParameter(ButtonParams& params) void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
{ {
Gwen::Controls::TextBox* label = new Gwen::Controls::TextBox(m_gwenInternalData->m_demoPage->GetPage());
Gwen::Controls::Button* button = new Gwen::Controls::Button(m_gwenInternalData->m_demoPage->GetPage()); Gwen::Controls::Button* button = new Gwen::Controls::Button(m_gwenInternalData->m_demoPage->GetPage());
MyButtonEventHandler* handler = new MyButtonEventHandler(params.m_callback,params.m_buttonId,params.m_userPointer); MyButtonEventHandler* handler = new MyButtonEventHandler(params.m_callback,params.m_buttonId,params.m_userPointer);
@@ -139,7 +138,9 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
m_paramInternalData->m_buttons.push_back(button); m_paramInternalData->m_buttons.push_back(button);
m_paramInternalData->m_buttonEventHandlers.push_back(handler); m_paramInternalData->m_buttonEventHandlers.push_back(handler);
button->SetPos( 10, m_gwenInternalData->m_curYposition ); button->SetPos( 5, m_gwenInternalData->m_curYposition );
button->SetWidth(120);
m_gwenInternalData->m_curYposition+=22; m_gwenInternalData->m_curYposition+=22;
} }

68
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -63,11 +63,18 @@ struct ImportUrdfInternalData
ImportUrdfInternalData() ImportUrdfInternalData()
:m_numMotors(0) :m_numMotors(0)
{ {
for (int i=0;i<MAX_NUM_MOTORS;i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
} }
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS]; btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS]; btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors; int m_numMotors;
}; };
@@ -91,7 +98,6 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
} else } else
{ {
gFileNameArray.clear(); gFileNameArray.clear();
gFileNameArray.push_back("r2d2.urdf");
@@ -116,6 +122,12 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
fclose(f); fclose(f);
} }
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
}
int numFileNames = gFileNameArray.size(); int numFileNames = gFileNameArray.size();
if (count>=numFileNames) if (count>=numFileNames)
@@ -210,9 +222,7 @@ void ImportUrdfSetup::initPhysics()
if (m_useMultiBody) if (m_useMultiBody)
{ {
//create motors for each btMultiBody joint
//create motors for each joint
for (int i=0;i<mb->getNumLinks();i++) for (int i=0;i<mb->getNumLinks();i++)
{ {
@@ -236,6 +246,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 0.1f; float maxMotorImpulse = 0.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor; m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor); m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++; m_data->m_numMotors++;
@@ -243,6 +254,44 @@ void ImportUrdfSetup::initPhysics()
} }
} }
} else
{
if (1)
{
//create motors for each generic joint
for (int i=0;i<creation.getNum6DofConstraints();i++)
{
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr())
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType ==URDFContinuousJoint))
{
int urdfLinkIndex = jointInfo->m_urdfIndex;
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
bool motorOn = true;
c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
m_data->m_numMotors++;
}
}
}
}
} }
} }
@@ -286,7 +335,16 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
{ {
for (int i=0;i<m_data->m_numMotors;i++) for (int i=0;i<m_data->m_numMotors;i++)
{ {
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]); if (m_data->m_jointMotors[i])
{
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
}
if (m_data->m_generic6DofJointMotors[i])
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
//jointInfo->
}
} }
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge

7
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -21,6 +21,13 @@ public:
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0; virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0;
virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0; virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0; virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;

137
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -9,7 +9,7 @@
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "URDFJointTypes.h"
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper) MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_guiHelper(guiHelper), :m_guiHelper(guiHelper),
@@ -42,12 +42,147 @@ class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider
} }
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
{ {
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder); btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
return c; return c;
} }
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
{
int rotateOrder=0;
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA , rbB, offsetInA, offsetInB, rotateOrder);
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
int principleAxis = jointAxisInJointSpace.closestAxis();
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
userInfo->m_jointAxisIndex = principleAxis;
userInfo->m_urdfJointType = URDFPrismaticJoint;
userInfo->m_lowerJointLimit = jointLowerLimit;
userInfo->m_upperJointLimit = jointUpperLimit;
userInfo->m_urdfIndex = urdfLinkIndex;
dof6->setUserConstraintPtr(userInfo);
switch (principleAxis)
{
case 0:
{
dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
break;
}
case 1:
{
dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
break;
}
case 2:
default:
{
dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
}
};
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_6DofConstraints.push_back(dof6);
return dof6;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
{
btGeneric6DofSpring2Constraint* dof6 = 0;
//only handle principle axis at the moment,
//@todo(erwincoumans) orient the constraint for non-principal axis
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_ZYX);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(-1,0,0));
dof6->setAngularLowerLimit(btVector3(1,0,0));
break;
}
case 1:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XZY);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,-1,0));
dof6->setAngularLowerLimit(btVector3(0,1,0));
break;
}
case 2:
default:
{
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XYZ);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,-1));
dof6->setAngularLowerLimit(btVector3(0,0,0));
}
};
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
userInfo->m_jointAxisIndex = 3+principleAxis;
if (jointLowerLimit > jointUpperLimit)
{
userInfo->m_urdfJointType = URDFContinuousJoint;
} else
{
userInfo->m_urdfJointType = URDFRevoluteJoint;
userInfo->m_lowerJointLimit = jointLowerLimit;
userInfo->m_upperJointLimit = jointUpperLimit;
userInfo->m_urdfIndex = urdfLinkIndex;
}
dof6->setUserConstraintPtr(userInfo);
m_6DofConstraints.push_back(dof6);
return dof6;
}
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB)
{
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB);
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
userInfo->m_urdfIndex = urdfLinkIndex;
userInfo->m_urdfJointType = URDFFixedJoint;
dof6->setUserConstraintPtr(userInfo);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
return dof6;
}
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex) void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
{ {
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex; // m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;

28
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -8,6 +8,17 @@
struct GUIHelperInterface; struct GUIHelperInterface;
class btMultiBody; class btMultiBody;
struct GenericConstraintUserInfo
{
int m_urdfIndex;
int m_urdfJointType;
btVector3 m_jointAxisInJointSpace;
int m_jointAxisIndex;
btScalar m_lowerJointLimit;
btScalar m_upperJointLimit;
};
class MyMultiBodyCreator : public MultiBodyCreationInterface class MyMultiBodyCreator : public MultiBodyCreationInterface
{ {
@@ -16,6 +27,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
public: public:
@@ -37,12 +49,28 @@ public:
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0); virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0);
virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body); virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex); virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody(); btMultiBody* getBulletMultiBody();
int getNum6DofConstraints() const
{
return m_6DofConstraints.size();
}
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
{
return m_6DofConstraints[index];
}
}; };
#endif //MY_MULTIBODY_CREATOR #endif //MY_MULTIBODY_CREATOR

87
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -296,19 +296,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{ {
printf("Fixed joint\n"); printf("Fixed joint\n");
btMatrix3x3 rm(offsetInA.getBasis()); btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
btScalar y,p,r;
rm.getEulerZYX(y,p,r);
printf("y=%f,p=%f,r=%f\n", y,p,r);
//we could also use btFixedConstraint but it has some issues
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
@@ -330,51 +318,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
//only handle principle axis at the moment,
//@todo(erwincoumans) orient the constraint for non-principal axis
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(-1,0,0)); btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
dof6->setAngularLowerLimit(btVector3(1,0,0));
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
break;
}
case 1:
{
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,-1,0));
dof6->setAngularLowerLimit(btVector3(0,1,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
break;
}
case 2:
default:
{
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,-1));
dof6->setAngularLowerLimit(btVector3(0,0,0));
if (enableConstraints)
world1->addConstraint(dof6,true);
}
};
printf("Revolute/Continuous joint\n"); printf("Revolute/Continuous joint\n");
} }
break; break;
@@ -396,33 +344,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
int principleAxis = jointAxisInJointSpace.closestAxis();
switch (principleAxis)
{
case 0:
{
dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
break;
}
case 1:
{
dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
break;
}
case 2:
default:
{
dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
}
};
dof6->setAngularLowerLimit(btVector3(0,0,0)); btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
dof6->setAngularUpperLimit(btVector3(0,0,0));
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);

78
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -6,7 +6,7 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h" #include "../CommonInterfaces/CommonMultiBodyBase.h"
btScalar radius(0.2);
struct TestJointTorqueSetup : public CommonMultiBodyBase struct TestJointTorqueSetup : public CommonMultiBodyBase
{ {
@@ -46,9 +46,12 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
} }
///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
void TestJointTorqueSetup::initPhysics() void TestJointTorqueSetup::initPhysics()
{ {
int upAxis = 1; int upAxis = 1;
gJointFeedbackInWorldSpace = true;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] = btVector4 colors[4] =
@@ -78,17 +81,23 @@ void TestJointTorqueSetup::initPhysics()
//create a static ground object //create a static ground object
if (1) if (1)
{ {
btVector3 groundHalfExtents(20,20,20); btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f; groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents); btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures(); box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box); m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity(); btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0); btVector3 groundOrigin(-0.4f, 3.f, 0.f);
groundOrigin[upAxis]=-1.5; btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin); start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box); btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor]; btVector4 color = colors[curColor];
curColor++; curColor++;
curColor&=3; curColor&=3;
@@ -97,9 +106,9 @@ void TestJointTorqueSetup::initPhysics()
{ {
bool floating = false; bool floating = false;
bool damping = true; bool damping = false;
bool gyro = false; bool gyro = false;
int numLinks = 1; int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false; bool canSleep = false;
bool selfCollide = false; bool selfCollide = false;
@@ -152,7 +161,14 @@ void TestJointTorqueSetup::initPhysics()
// linkMass= 1000; // linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f); btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//new btSphereShape(linkHalfExtents[0]); btCollisionShape* shape = 0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
} else
{
shape = new btSphereShape(radius);
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag); shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape; delete shape;
@@ -161,11 +177,25 @@ void TestJointTorqueSetup::initPhysics()
{ {
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f), btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis, hingeJointAxis,
parentComToCurrentPivot, parentComToCurrentPivot,
currentPivotToCurrentCom, false); currentPivotToCurrentCom, false);
} else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -205,10 +235,8 @@ void TestJointTorqueSetup::initPhysics()
mbC->setAngularDamping(0.9f); mbC->setAngularDamping(0.9f);
} }
// //
btVector3 gravity(0,0,0); m_dynamicsWorld->setGravity(btVector3(0,0,-10));
gravity[upAxis] = -9.81;
gravity[0] = 0;
m_dynamicsWorld->setGravity(gravity);
////////////////////////////////////////////// //////////////////////////////////////////////
if(0)//numLinks > 0) if(0)//numLinks > 0)
{ {
@@ -255,7 +283,9 @@ void TestJointTorqueSetup::initPhysics()
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]); tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr); col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating); bool isDynamic = (baseMass > 0 && floating);
@@ -268,7 +298,7 @@ void TestJointTorqueSetup::initPhysics()
btVector3 color(0.0,0.0,0.5); btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color); m_guiHelper->createCollisionObjectGraphicsObject(col,color);
col->setFriction(friction); // col->setFriction(friction);
pMultiBody->setBaseCollider(col); pMultiBody->setBaseCollider(col);
} }
@@ -290,8 +320,17 @@ void TestJointTorqueSetup::initPhysics()
// float pos[4]={posr.x(),posr.y(),posr.z(),1}; // float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
{
shape = new btSphereShape(radius);
}
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape); m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
@@ -301,7 +340,7 @@ void TestJointTorqueSetup::initPhysics()
tr.setOrigin(posr); tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr); col->setWorldTransform(tr);
col->setFriction(friction); // col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0); bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
@@ -335,9 +374,12 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
} }
m_dynamicsWorld->stepSimulation(1./60,0); m_dynamicsWorld->stepSimulation(1./240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
if (1)
for (int i=0;i<m_jointFeedbacks.size();i++) for (int i=0;i<m_jointFeedbacks.size();i++)
{ {
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f", b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
@@ -353,6 +395,8 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
); );
} }
}
count++;
/* /*

265
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -10,6 +10,7 @@
class PhysicsClient : public SharedMemoryCommon class PhysicsClient : public SharedMemoryCommon
{ {
protected:
SharedMemoryInterface* m_sharedMemory; SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1; SharedMemoryExampleData* m_testBlock1;
int m_counter; int m_counter;
@@ -19,7 +20,8 @@ class PhysicsClient : public SharedMemoryCommon
bool m_serverLoadUrdfOK; bool m_serverLoadUrdfOK;
bool m_waitingForServer; bool m_waitingForServer;
void processServerCommands();
void createClientCommand();
public: public:
@@ -59,7 +61,11 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
cl->submitCommand(CMD_LOAD_URDF); cl->submitCommand(CMD_LOAD_URDF);
break; break;
} }
case CMD_REQUEST_ACTUAL_STATE:
{
cl->submitCommand(CMD_REQUEST_ACTUAL_STATE);
break;
}
case CMD_STEP_FORWARD_SIMULATION: case CMD_STEP_FORWARD_SIMULATION:
{ {
cl->submitCommand(CMD_STEP_FORWARD_SIMULATION); cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
@@ -95,7 +101,7 @@ m_wantsTermination(false),
m_serverLoadUrdfOK(false), m_serverLoadUrdfOK(false),
m_waitingForServer(false) m_waitingForServer(false)
{ {
b3Printf("Started PhysicsClient"); b3Printf("Started PhysicsClient\n");
#ifdef _WIN32 #ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryClient(); m_sharedMemory = new Win32SharedMemoryClient();
#else #else
@@ -105,7 +111,7 @@ m_waitingForServer(false)
PhysicsClient::~PhysicsClient() PhysicsClient::~PhysicsClient()
{ {
b3Printf("~PhysicsClient"); b3Printf("~PhysicsClient\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
delete m_sharedMemory; delete m_sharedMemory;
} }
@@ -113,29 +119,61 @@ PhysicsClient::~PhysicsClient()
void PhysicsClient::initPhysics() void PhysicsClient::initPhysics()
{ {
if (m_guiHelper && m_guiHelper->getParameterInterface())
{ {
bool isTrigger = false; {
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger); bool isTrigger = false;
button.m_callback = MyCallback; ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_userPointer = this; button.m_callback = MyCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter(button); button.m_userPointer = this;
} m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{ {
bool isTrigger = false; bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback; button.m_callback = MyCallback;
button.m_userPointer = this; button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button); m_guiHelper->getParameterInterface()->registerButtonParameter(button);
} }
{
bool isTrigger = false;
ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
} else
{ {
bool isTrigger = false; m_userCommandRequests.push_back(CMD_LOAD_URDF);
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
button.m_callback = MyCallback; //m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
button.m_userPointer = this; m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_guiHelper->getParameterInterface()->registerButtonParameter(button); //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);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
@@ -144,79 +182,119 @@ void PhysicsClient::initPhysics()
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER); // btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{ {
b3Error("Error: please start server before client"); b3Error("Error: please start server before client\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0; m_testBlock1 = 0;
} else } else
{ {
b3Printf("Shared Memory status is OK"); b3Printf("Shared Memory status is OK\n");
} }
} } else
{
m_wantsTermination = true;
}
} }
void PhysicsClient::stepSimulation(float deltaTime) void PhysicsClient::processServerCommands()
{ {
btAssert(m_testBlock1);
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
if (m_testBlock1) const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
{
//check progress and submit further commands
//we ignore overflow right now
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands) //consume the command
{ switch (serverCmd.m_type)
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1); {
const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; case CMD_URDF_LOADING_COMPLETED:
{
//consume the command m_serverLoadUrdfOK = true;
switch (serverCmd.m_type) b3Printf("Server loading the URDF OK\n");
{ break;
}
case CMD_URDF_LOADING_COMPLETED: case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
break;
}
case CMD_URDF_LOADING_FAILED:
{
b3Printf("Server failed loading the URDF...\n");
m_serverLoadUrdfOK = false;
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{ {
m_serverLoadUrdfOK = true; b3Printf("Received actual state\n");
b3Printf("Server loading the URDF OK");
int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
char msg[1024];
sprintf(msg,"Q=[");
for (int i=0;i<numQ;i++)
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]);
}
}
sprintf(msg,"%s]",msg);
b3Printf(msg);
b3Printf("\n");
break; break;
} }
case CMD_STEP_FORWARD_SIMULATION_COMPLETED: default:
{
break;
}
case CMD_URDF_LOADING_FAILED:
{
b3Printf("Server failed loading the URDF...");
m_serverLoadUrdfOK = false;
break;
}
default:
{
b3Error("Unknown server command");
btAssert(0);
}
};
m_testBlock1->m_numProcessedServerCommands++;
if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
{ {
m_waitingForServer = false; b3Error("Unknown server command\n");
} else btAssert(0);
{
m_waitingForServer = true;
} }
} };
if (!m_waitingForServer)
m_testBlock1->m_numProcessedServerCommands++;
//we don't have more than 1 command outstanding (in total, either server or client)
btAssert(m_testBlock1->m_numProcessedServerCommands == m_testBlock1->m_numServerCommands);
if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
{
m_waitingForServer = false;
} else
{
m_waitingForServer = true;
}
}
}
void PhysicsClient::createClientCommand()
{
if (!m_waitingForServer)
{ {
//process outstanding requests //process outstanding requests
if (m_userCommandRequests.size()) if (m_userCommandRequests.size())
{ {
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int command = m_userCommandRequests[0]; int command = m_userCommandRequests[0];
m_userCommandRequests.remove(command);
//don't use 'remove' because it will re-order the commands
//m_userCommandRequests.remove(command);
//pop_front
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
m_waitingForServer = true;
switch (command) switch (command)
{ {
case CMD_LOAD_URDF: case CMD_LOAD_URDF:
@@ -229,11 +307,24 @@ void PhysicsClient::stepSimulation(float deltaTime)
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true; m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_testBlock1->m_numClientCommands++; m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF"); b3Printf("Client created CMD_LOAD_URDF\n");
m_waitingForServer = true;
} else } else
{ {
b3Warning("Server already loaded URDF, no client command submitted"); b3Warning("Server already loaded URDF, no client command submitted\n");
}
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_serverLoadUrdfOK)
{
b3Printf("Requesting actual state\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("No URDF loaded\n");
} }
break; break;
} }
@@ -242,14 +333,13 @@ void PhysicsClient::stepSimulation(float deltaTime)
if (m_serverLoadUrdfOK) if (m_serverLoadUrdfOK)
{ {
m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_testBlock1->m_numClientCommands++; m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++); b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
m_waitingForServer = true;
} else } else
{ {
b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted"); b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
} }
break; break;
} }
@@ -258,20 +348,33 @@ void PhysicsClient::stepSimulation(float deltaTime)
m_wantsTermination = true; m_wantsTermination = true;
m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_testBlock1->m_numClientCommands++; m_testBlock1->m_numClientCommands++;
m_waitingForServer = true;
m_serverLoadUrdfOK = false; m_serverLoadUrdfOK = false;
b3Printf("client created CMD_SHUTDOWN\n"); b3Printf("client created CMD_SHUTDOWN\n");
break; break;
} }
default: default:
{ {
b3Error("unknown command requested"); b3Error("unknown command requested\n");
} }
} }
} }
} }
} }
void PhysicsClient::stepSimulation(float deltaTime)
{
btAssert(m_testBlock1);
if (m_testBlock1)
{
processServerCommands();
if (!m_waitingForServer)
{
createClientCommand();
}
}
} }

99
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -62,8 +62,10 @@ m_wantsShutdown(false)
void PhysicsServer::releaseSharedMemory() void PhysicsServer::releaseSharedMemory()
{ {
b3Printf("releaseSharedMemory1\n");
if (m_testBlock1) if (m_testBlock1)
{ {
b3Printf("m_testBlock1\n");
m_testBlock1->m_magicId = 0; m_testBlock1->m_magicId = 0;
b3Printf("magic id = %d\n",m_testBlock1->m_magicId); b3Printf("magic id = %d\n",m_testBlock1->m_magicId);
btAssert(m_sharedMemory); btAssert(m_sharedMemory);
@@ -71,7 +73,7 @@ void PhysicsServer::releaseSharedMemory()
} }
if (m_sharedMemory) if (m_sharedMemory)
{ {
b3Printf("m_sharedMemory\n");
delete m_sharedMemory; delete m_sharedMemory;
m_sharedMemory = 0; m_sharedMemory = 0;
m_testBlock1 = 0; m_testBlock1 = 0;
@@ -217,19 +219,75 @@ void PhysicsServer::stepSimulation(float deltaTime)
m_testBlock1->m_numServerCommands++; m_testBlock1->m_numServerCommands++;
break; break;
} }
case CMD_STEP_FORWARD_SIMULATION: case CMD_REQUEST_ACTUAL_STATE:
{ {
b3Printf("Sending the actual state (Q,U)");
if (m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0);
SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0];
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
b3Printf("Step simulation request"); serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds; int totalDegreeOfFreedomQ = 0;
m_dynamicsWorld->stepSimulation(timeStep); int totalDegreeOfFreedomU = 0;
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; //always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
btTransform tr;
tr.setOrigin(mb->getBasePos());
tr.setRotation(mb->getWorldToBaseRot().inverse());
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; //base position in world space, carthesian
m_testBlock1->m_numServerCommands++; m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
//now we send back the actual q, q' and force/torque and IMU sensor values //base orientation, quaternion x,y,z,w, in world space, carthesian
m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0];
m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
}
for (int d=0;d<mb->getLink(l).m_dofCount;d++)
{
m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
} else
{
b3Warning("Request state but no multibody available");
//rigid bodies?
}
/*
//now we send back the actual q, q' and force/torque and IMU sensor values
for (int i=0;i<m_jointFeedbacks.size();i++) for (int i=0;i<m_jointFeedbacks.size();i++)
{ {
printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n", printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
@@ -246,6 +304,25 @@ void PhysicsServer::stepSimulation(float deltaTime)
m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(), m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(),
m_jointFeedbacks[i]->m_appliedTorqueBodyB.z()); m_jointFeedbacks[i]->m_appliedTorqueBodyB.z());
} }
*/
m_testBlock1->m_numServerCommands++;
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3Printf("Step simulation request");
double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
m_dynamicsWorld->stepSimulation(timeStep);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_testBlock1->m_numServerCommands++;
break; break;
} }
case CMD_SHUTDOWN: case CMD_SHUTDOWN:
@@ -268,6 +345,8 @@ void PhysicsServer::stepSimulation(float deltaTime)
} }
if (wantsShutdown) if (wantsShutdown)
{ {
b3Printf("releaseSharedMemory!\n");
m_wantsShutdown = true; m_wantsShutdown = true;
releaseSharedMemory(); releaseSharedMemory();
} }

107
examples/SharedMemory/SharedMemoryCommands.h Normal file
View File

@@ -0,0 +1,107 @@
#ifndef SHARED_MEMORY_COMMANDS_H
#define SHARED_MEMORY_COMMANDS_H
//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
enum SharedMemoryServerCommand
{
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
CMD_RIGID_BODY_CREATION_COMPLETED,
CMD_SET_JOINT_FEEDBACK_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_COMPLETED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS
};
enum SharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_CREATE_BOX_COLLISION_SHAPE,
CMD_DELETE_BOX_COLLISION_SHAPE,
CMD_CREATE_RIGID_BODY,
CMD_DELETE_RIGID_BODY,
CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback
CMD_SEND_DESIRED_STATE,
CMD_REQUEST_ACTUAL_STATE,
CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE
CMD_SHUTDOWN,
CMD_MAX_CLIENT_COMMANDS
};
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 1024
#define MAX_NUM_SENSORS 1024
#define MAX_URDF_FILENAME_LENGTH 1024
struct UrdfArgs
{
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
bool m_useMultiBody;
bool m_useFixedBase;
};
struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
int m_linkId;
bool m_isEnabled;
};
struct SendDesiredStateArgs
{
int m_bodyUniqueId;
};
struct RequestActualStateArgs
{
int m_bodyUniqueId;
};
struct CreateBoxShapeArgs
{
double m_halfExtents[3];
};
struct CreateRigidBodyArgs
{
int m_shapeId;
double m_initialPosition[3];
double m_initialOrientation[3];
};
struct SendActualStateArgs
{
int m_bodyUniqueId;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
};
struct StepSimulationArgs
{
double m_deltaTimeInSeconds;
};
struct SharedMemoryCommand
{
int m_type;
union
{
UrdfArgs m_urdfArguments;
StepSimulationArgs m_stepSimulationArguments;
SendDesiredStateArgs m_sendDesiredQandUStateCommandArgument;
RequestActualStateArgs m_requestActualStateInformationCommandArgument;
SendActualStateArgs m_sendActualStateArgs;
};
};
#endif //SHARED_MEMORY_COMMANDS_H

50
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -5,51 +5,7 @@
#define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 64 #define SHARED_MEMORY_MAX_COMMANDS 64
enum SharedMemoryServerCommand{ #include "SharedMemoryCommands.h"
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_SERVER_STATE_UPDATE_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS
};
enum SharedMemoryClientCommand{
CMD_LOAD_URDF,
CMD_STATE_UPDATED,
CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE
CMD_SHUTDOWN,
CMD_MAX_CLIENT_COMMANDS
};
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 1024
#define MAX_NUM_SENSORS 1024
#define MAX_URDF_FILENAME_LENGTH 1024
struct UrdfCommandArgument
{
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
bool m_useMultiBody;
bool m_useFixedBase;
};
struct StepSimulationCommandArgument
{
double m_deltaTimeInSeconds;
};
struct SharedMemoryCommand
{
int m_type;
union
{
UrdfCommandArgument m_urdfArguments;
StepSimulationCommandArgument m_stepSimulationArguments;
};
};
struct SharedMemoryExampleData struct SharedMemoryExampleData
{ {
@@ -63,18 +19,18 @@ struct SharedMemoryExampleData
int m_numServerCommands; int m_numServerCommands;
int m_numProcessedServerCommands; int m_numProcessedServerCommands;
//TODO: it is better to move this single desired/actual state (m_desiredStateQ, m_actualStateQ etc) into the command,
//so we can deal with multiple bodies/robots
//desired state is only written by the client, read-only access by server is expected //desired state is only written by the client, read-only access by server is expected
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
//actual state is only written by the server, read-only access by client is expected //actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
}; };
#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData) #define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)

34
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -30,6 +30,10 @@
#include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM // #define INCLUDE_GYRO_TERM
///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
bool gJointFeedbackInWorldSpace = false;
bool gJointFeedbackInJointFrame = false;
namespace { namespace {
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
@@ -1023,13 +1027,30 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
if (m_links[i].m_jointFeedback) if (m_links[i].m_jointFeedback)
{ {
m_internalNeedsJointFeedback = true; m_internalNeedsJointFeedback = true;
m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
}
if (gJointFeedbackInJointFrame)
{
//shift the reaction forces to the joint frame
//linear (force) component is the same
//shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
}
if (gJointFeedbackInWorldSpace)
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
} else
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
}
}
} }
@@ -2308,7 +2329,6 @@ void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to
for (int i = 0; i < num_links; ++i) for (int i = 0; i < num_links; ++i)
{ {
const int parent = m_links[i].m_parent;
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
} }

1
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -616,7 +616,6 @@ private:
btAlignedObjectArray<btVector3> m_vectorBuf; btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf; btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 m_cachedInertiaTopLeft; btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight; btMatrix3x3 m_cachedInertiaTopRight;

1
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -895,7 +895,6 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (c.m_multiBodyA) if (c.m_multiBodyA)
{ {
btScalar ai = c.m_appliedImpulse;
if(c.m_multiBodyA->isMultiDof()) if(c.m_multiBodyA->isMultiDof())
{ {

11
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -838,11 +838,9 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
int nLinks = bod->getNumLinks();
for (int m = 0; m<bod->getNumLinks(); m++) for (int m = 0; m<bod->getNumLinks(); m++)
{ {
int link = m;
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
@@ -858,6 +856,15 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color); getDebugDrawer()->drawLine(from,to,color);
} }
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{ {
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);

1
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
View File

@@ -22,7 +22,6 @@ subject to the following restrictions:
struct btMultiBodyJointFeedback struct btMultiBodyJointFeedback
{ {
btSpatialForceVector m_reactionForces; btSpatialForceVector m_reactionForces;
btSymmetricSpatialDyad m_spatialInertia;
}; };
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H #endif //BT_MULTIBODY_JOINT_FEEDBACK_H

1
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -26,7 +26,6 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredVelocity(desiredVelocity) m_desiredVelocity(desiredVelocity)
{ {
int parent = body->getLink(link).m_parent;
m_maxAppliedImpulse = maxMotorImpulse; m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well // the data.m_jacobians never change, so may as well
// initialize them here // initialize them here