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

enable motor control for maximal coordinates robots (btRigidBody, btTypedConstraint) for force, velocity, position control.

Browse Source
This commit is contained in:
erwincoumans
2017-08-29 19:14:27 -07:00
parent 4ff6befc6d
commit 1f7db4519e
5 changed files with 386 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -180,7 +180,7 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int u
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_6DofConstraints.push_back(dof6);
return dof6;
}
@@ -188,6 +188,10 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int u
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
{
if (m_mb2urdfLink.size()<(mbLinkIndex+1))
{
m_mb2urdfLink.resize((mbLinkIndex+1),-2);
}
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
}

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

@@ -378,12 +378,14 @@ void ConvertURDF2BulletInternal(
{
printf("Warning: joint unsupported, creating a fixed joint instead.");
}
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else
{
//b3Printf("Fixed joint\n");
@@ -398,13 +400,14 @@ void ConvertURDF2BulletInternal(
case URDFContinuousJoint:
case URDFRevoluteJoint:
{
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (createMultiBody)
{
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
@@ -424,13 +427,15 @@ void ConvertURDF2BulletInternal(
}
case URDFPrismaticJoint:
{
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (createMultiBody)
{
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
@@ -444,7 +449,8 @@ void ConvertURDF2BulletInternal(
} else
{
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
//btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints)