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"/>
<origin xyz="0 0 1.0"/>
</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>

164
examples/Constraints/TestHingeTorque.cpp
View File

@@ -5,8 +5,8 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
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
{
@@ -50,7 +50,7 @@ TestHingeTorque::~ TestHingeTorque()
void TestHingeTorque::stepSimulation(float deltaTime)
{
if (m_once)
if (0)//m_once)
{
m_once=false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
@@ -63,37 +63,41 @@ void TestHingeTorque::stepSimulation(float deltaTime)
}
m_dynamicsWorld->stepSimulation(1./60,0);
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
base->getAngularVelocity()[1],
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++)
m_dynamicsWorld->stepSimulation(1./240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
m_jointFeedback[i]->m_appliedForceBodyA.x(),
m_jointFeedback[i]->m_appliedForceBodyA.y(),
m_jointFeedback[i]->m_appliedForceBodyA.z(),
m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
m_jointFeedback[i]->m_appliedTorqueBodyA.y(),
m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
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());
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
base->getAngularVelocity()[1],
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 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);
}
@@ -101,10 +105,13 @@ void TestHingeTorque::stepSimulation(float deltaTime)
void TestHingeTorque::initPhysics()
{
m_guiHelper->setUpAxis(1);
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
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);
int mode = btIDebugDraw::DBG_DrawWireframe
@@ -115,7 +122,7 @@ void TestHingeTorque::initPhysics()
{ // 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 canSleep = false;
bool selfCollide = false;
@@ -138,7 +145,9 @@ void TestHingeTorque::initPhysics()
m_dynamicsWorld->removeRigidBody(base);
base->setDamping(0,0);
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
btBoxShape* linkBox = new btBoxShape(linkHalfExtents);
btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
btSphereShape* linkSphere = new btSphereShape(radius);
btRigidBody* prevBody = base;
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));
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->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
linkBody->setDamping(0,0);
//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);
btJointFeedback* fb = new btJointFeedback();
m_jointFeedback.push_back(fb);
hinge->setJointFeedback(fb);
btTypedConstraint* con = 0;
if (i==0)
{
//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;
m_dynamicsWorld->addConstraint(hinge,true);
prevBody = linkBody;
}
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);
}

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

@@ -129,7 +129,6 @@ void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue)
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());
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_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;
}

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

@@ -63,11 +63,18 @@ struct ImportUrdfInternalData
ImportUrdfInternalData()
: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];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
};
@@ -91,8 +98,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
} else
{
gFileNameArray.clear();
gFileNameArray.push_back("r2d2.urdf");
//load additional urdf file names from file
@@ -115,6 +121,12 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
fclose(f);
}
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
}
int numFileNames = gFileNameArray.size();
@@ -210,9 +222,7 @@ void ImportUrdfSetup::initPhysics()
if (m_useMultiBody)
{
//create motors for each joint
//create motors for each btMultiBody joint
for (int i=0;i<mb->getNumLinks();i++)
{
@@ -236,6 +246,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 0.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
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++)
{
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

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* 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 void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;

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

@@ -9,7 +9,7 @@
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "URDFJointTypes.h"
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* 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)
{
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
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)
{
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;

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

@@ -8,6 +8,17 @@
struct GUIHelperInterface;
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
{
@@ -16,6 +27,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
struct GUIHelperInterface* m_guiHelper;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
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* 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 void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody();
int getNum6DofConstraints() const
{
return m_6DofConstraints.size();
}
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
{
return m_6DofConstraints[index];
}
};
#endif //MY_MULTIBODY_CREATOR

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

@@ -295,21 +295,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else
{
printf("Fixed joint\n");
btMatrix3x3 rm(offsetInA.getBasis());
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));
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
if (enableConstraints)
world1->addConstraint(dof6,true);
}
@@ -330,51 +318,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} 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));
dof6->setAngularLowerLimit(btVector3(1,0,0));
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
if (enableConstraints)
if (enableConstraints)
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");
}
break;
@@ -396,33 +344,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} 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));
dof6->setAngularUpperLimit(btVector3(0,0,0));
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints)
world1->addConstraint(dof6,true);

78
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -6,7 +6,7 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h"
btScalar radius(0.2);
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()
{
int upAxis = 1;
gJointFeedbackInWorldSpace = true;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
@@ -78,17 +81,23 @@ void TestJointTorqueSetup::initPhysics()
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(20,20,20);
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
@@ -97,9 +106,9 @@ void TestJointTorqueSetup::initPhysics()
{
bool floating = false;
bool damping = true;
bool damping = 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 canSleep = false;
bool selfCollide = false;
@@ -152,7 +161,14 @@ void TestJointTorqueSetup::initPhysics()
// linkMass= 1000;
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);
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);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
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);
@@ -205,10 +235,8 @@ void TestJointTorqueSetup::initPhysics()
mbC->setAngularDamping(0.9f);
}
//
btVector3 gravity(0,0,0);
gravity[upAxis] = -9.81;
gravity[0] = 0;
m_dynamicsWorld->setGravity(gravity);
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
//////////////////////////////////////////////
if(0)//numLinks > 0)
{
@@ -255,7 +283,9 @@ void TestJointTorqueSetup::initPhysics()
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
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);
bool isDynamic = (baseMass > 0 && floating);
@@ -268,7 +298,7 @@ void TestJointTorqueSetup::initPhysics()
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
col->setFriction(friction);
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
@@ -290,8 +320,17 @@ void TestJointTorqueSetup::initPhysics()
// 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()};
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);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
@@ -301,7 +340,7 @@ void TestJointTorqueSetup::initPhysics()
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
// col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(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]);
}
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++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
@@ -353,6 +395,8 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
);
}
}
count++;
/*

273
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -10,6 +10,7 @@
class PhysicsClient : public SharedMemoryCommon
{
protected:
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
int m_counter;
@@ -19,7 +20,8 @@ class PhysicsClient : public SharedMemoryCommon
bool m_serverLoadUrdfOK;
bool m_waitingForServer;
void processServerCommands();
void createClientCommand();
public:
@@ -59,7 +61,11 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
cl->submitCommand(CMD_LOAD_URDF);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
cl->submitCommand(CMD_REQUEST_ACTUAL_STATE);
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
@@ -95,7 +101,7 @@ m_wantsTermination(false),
m_serverLoadUrdfOK(false),
m_waitingForServer(false)
{
b3Printf("Started PhysicsClient");
b3Printf("Started PhysicsClient\n");
#ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryClient();
#else
@@ -105,7 +111,7 @@ m_waitingForServer(false)
PhysicsClient::~PhysicsClient()
{
b3Printf("~PhysicsClient");
b3Printf("~PhysicsClient\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
delete m_sharedMemory;
}
@@ -113,29 +119,61 @@ PhysicsClient::~PhysicsClient()
void PhysicsClient::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
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;
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
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);
}
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);
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_testBlock1 = 0;
} 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()
{
if (m_testBlock1)
{
//check progress and submit further commands
//we ignore overflow right now
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
//consume the command
switch (serverCmd.m_type)
{
btAssert(m_testBlock1);
case CMD_URDF_LOADING_COMPLETED:
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
//consume the command
switch (serverCmd.m_type)
{
case CMD_URDF_LOADING_COMPLETED:
{
m_serverLoadUrdfOK = true;
b3Printf("Server loading the URDF OK\n");
break;
}
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("Server loading the URDF OK");
b3Printf("Received actual state\n");
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;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
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)
default:
{
m_waitingForServer = false;
} else
{
m_waitingForServer = true;
b3Error("Unknown server command\n");
btAssert(0);
}
}
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
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
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)
{
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_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF");
m_waitingForServer = true;
b3Printf("Client created CMD_LOAD_URDF\n");
} 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;
}
@@ -242,14 +333,13 @@ void PhysicsClient::stepSimulation(float deltaTime)
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_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
m_waitingForServer = true;
} 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;
}
@@ -258,20 +348,33 @@ void PhysicsClient::stepSimulation(float deltaTime)
m_wantsTermination = true;
m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_testBlock1->m_numClientCommands++;
m_waitingForServer = true;
m_serverLoadUrdfOK = false;
b3Printf("client created CMD_SHUTDOWN\n");
break;
}
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();
}
}
}

105
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -62,8 +62,10 @@ m_wantsShutdown(false)
void PhysicsServer::releaseSharedMemory()
{
b3Printf("releaseSharedMemory1\n");
if (m_testBlock1)
{
b3Printf("m_testBlock1\n");
m_testBlock1->m_magicId = 0;
b3Printf("magic id = %d\n",m_testBlock1->m_magicId);
btAssert(m_sharedMemory);
@@ -71,7 +73,7 @@ void PhysicsServer::releaseSharedMemory()
}
if (m_sharedMemory)
{
b3Printf("m_sharedMemory\n");
delete m_sharedMemory;
m_sharedMemory = 0;
m_testBlock1 = 0;
@@ -217,19 +219,75 @@ void PhysicsServer::stepSimulation(float deltaTime)
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++;
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;
//now we send back the actual q, q' and force/torque and IMU sensor values
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 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());
//base position in world space, carthesian
m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
//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++)
{
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.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;
}
case CMD_SHUTDOWN:
@@ -268,6 +345,8 @@ void PhysicsServer::stepSimulation(float deltaTime)
}
if (wantsShutdown)
{
b3Printf("releaseSharedMemory!\n");
m_wantsShutdown = true;
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

56
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -5,51 +5,7 @@
#define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 64
enum SharedMemoryServerCommand{
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;
};
};
#include "SharedMemoryCommands.h"
struct SharedMemoryExampleData
{
@@ -62,19 +18,19 @@ struct SharedMemoryExampleData
int m_numServerCommands;
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
double m_desiredStateQ[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
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
};
#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)

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

@@ -30,6 +30,10 @@
#include "Bullet3Common/b3Logging.h"
// #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 {
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
@@ -1023,13 +1027,30 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
if (m_links[i].m_jointFeedback)
{
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;
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 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
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)
{
const int parent = m_links[i].m_parent;
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<btMatrix3x3> m_matrixBuf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight;

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

@@ -895,7 +895,6 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (c.m_multiBodyA)
{
btScalar ai = c.m_appliedImpulse;
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);
int nLinks = bod->getNumLinks();
for (int m = 0; m<bod->getNumLinks(); m++)
{
int link = m;
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);
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)
{
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
{
btSpatialForceVector m_reactionForces;
btSymmetricSpatialDyad m_spatialInertia;
};
#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)
{
int parent = body->getLink(link).m_parent;
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here