diff --git a/data/sphere2.urdf b/data/sphere2.urdf
index 35bf786a6..891e018cb 100644
--- a/data/sphere2.urdf
+++ b/data/sphere2.urdf
@@ -43,5 +43,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/Constraints/TestHingeTorque.cpp b/examples/Constraints/TestHingeTorque.cpp
index 8afec175c..bdc9c2f6a 100644
--- a/examples/Constraints/TestHingeTorque.cpp
+++ b/examples/Constraints/TestHingeTorque.cpp
@@ -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;istepSimulation(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;im_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;iremoveRigidBody(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);
}
diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
index 322fca33c..4389f9e03 100644
--- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
@@ -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;
}
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index fbcde7d9b..636460bae 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -63,11 +63,18 @@ struct ImportUrdfInternalData
ImportUrdfInternalData()
:m_numMotors(0)
{
+ for (int i=0;igetNumLinks();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;igetUserConstraintPtr())
+ {
+ 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;im_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
diff --git a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
index 04cd5bd93..2cbc2f5e2 100644
--- a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
+++ b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
@@ -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;
diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
index 60226271b..aa249d538 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
@@ -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;
diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
index 71b2f18f2..99dbc9fc9 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
@@ -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 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
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 92a0b24db..7b05fc7b3 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -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);
diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp
index bd5b0e10a..300ced7db 100644
--- a/examples/MultiBody/TestJointTorqueSetup.cpp
+++ b/examples/MultiBody/TestJointTorqueSetup.cpp
@@ -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;isubmitCommand(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;im_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;im_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();
+ }
+ }
}
diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp
index 1f0165d4f..6a7b65b55 100644
--- a/examples/SharedMemory/PhysicsServer.cpp
+++ b/examples/SharedMemory/PhysicsServer.cpp
@@ -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;lgetNumLinks();l++)
+ {
+ for (int d=0;dgetLink(l).m_posVarCount;d++)
+ {
+ m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
+ }
+ for (int d=0;dgetLink(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;im_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();
}
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
new file mode 100644
index 000000000..6c80570ba
--- /dev/null
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -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
diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h
index c69dad4ec..6a31a6e94 100644
--- a/examples/SharedMemory/SharedMemoryInterface.h
+++ b/examples/SharedMemory/SharedMemoryInterface.h
@@ -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)
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 7225dfecf..56f6bdfc2 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -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& 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);
}
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index cc1b3ee76..d136c7bd5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -616,7 +616,6 @@ private:
btAlignedObjectArray m_vectorBuf;
btAlignedObjectArray m_matrixBuf;
- //std::auto_ptr > > cached_imatrix_lu;
btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 33a01bb23..b4a97c8b8 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -895,7 +895,6 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (c.m_multiBodyA)
{
- btScalar ai = c.m_appliedImpulse;
if(c.m_multiBodyA->isMultiDof())
{
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index ffc66d9c2..579cb47f5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -838,11 +838,9 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
- int nLinks = bod->getNumLinks();
for (int m = 0; mgetNumLinks(); 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);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
index fe2ef15e8..5c2fa8ed5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
@@ -22,7 +22,6 @@ subject to the following restrictions:
struct btMultiBodyJointFeedback
{
btSpatialForceVector m_reactionForces;
- btSymmetricSpatialDyad m_spatialInertia;
};
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index f2912cdd0..be7aff045 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -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