AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-04 17:40:06 +00:00
Code Issues Releases Wiki Activity

add infrastructure float btMultiBodyLink m_jointDamping, m_jointFriction (actual damping/friction is in a separate commit)

Browse Source 
add door.urdf for testing damping/friction
This commit is contained in:
erwincoumans 2016-03-17 14:54:46 -07:00
parent 937b6d84e5
commit fe92de3e50
15 changed files with 1005 additions and 874 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

159
Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
View File

@ -67,6 +67,8 @@ typedef struct bInvalidHandle {
class btConvexHullShapeData;
class btCollisionObjectDoubleData;
class btCollisionObjectFloatData;
class btContactSolverInfoDoubleData;
class btContactSolverInfoFloatData;
class btDynamicsWorldDoubleData;
class btDynamicsWorldFloatData;
class btRigidBodyFloatData;
@ -93,8 +95,6 @@ typedef struct bInvalidHandle {
class btSliderConstraintDoubleData;
class btGearConstraintFloatData;
class btGearConstraintDoubleData;
class btContactSolverInfoDoubleData;
class btContactSolverInfoFloatData;
class SoftBodyMaterialData;
class SoftBodyNodeData;
class SoftBodyLinkData;
@ -600,6 +600,81 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btContactSolverInfoDoubleData
{
public:
double m_tau;
double m_damping;
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;
double m_erp2;
double m_globalCfm;
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
// -------------------------------------------------- //
class btContactSolverInfoFloatData
{
public:
float m_tau;
float m_damping;
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;
float m_erp2;
float m_globalCfm;
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
// -------------------------------------------------- //
class btDynamicsWorldDoubleData
{
public:
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
};
// -------------------------------------------------- //
class btDynamicsWorldFloatData
{
public:
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
};
// -------------------------------------------------- //
class btRigidBodyFloatData
@ -1061,82 +1136,6 @@ typedef struct bInvalidHandle {
};
// -------------------------------------------------- //
class btContactSolverInfoDoubleData
{
public:
double m_tau;
double m_damping;
double m_friction;
double m_timeStep;
double m_restitution;
double m_maxErrorReduction;
double m_sor;
double m_erp;
double m_erp2;
double m_globalCfm;
double m_splitImpulsePenetrationThreshold;
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_maxGyroscopicForce;
double m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
// -------------------------------------------------- //
class btContactSolverInfoFloatData
{
public:
float m_tau;
float m_damping;
float m_friction;
float m_timeStep;
float m_restitution;
float m_maxErrorReduction;
float m_sor;
float m_erp;
float m_erp2;
float m_globalCfm;
float m_splitImpulsePenetrationThreshold;
float m_splitImpulseTurnErp;
float m_linearSlop;
float m_warmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
// -------------------------------------------------- //
class btDynamicsWorldDoubleData
{
public:
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
};
// -------------------------------------------------- //
class btDynamicsWorldFloatData
{
public:
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
};
// -------------------------------------------------- //
class SoftBodyMaterialData
{
@ -1366,6 +1365,8 @@ typedef struct bInvalidHandle {
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
double m_jointDamping;
double m_jointFriction;
};
@ -1390,6 +1391,8 @@ typedef struct bInvalidHandle {
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
float m_jointDamping;
float m_jointFriction;
};

98
data/door.urdf Normal file
View File

@ -0,0 +1,98 @@
<?xml version="0.0" ?>
<robot name="urdf_door">
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.35"/>
<mass value="0.0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.05 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="framemat0">
<color
rgba="0.9 0.4 0. 1" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.95 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.5 0 0.95"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.5 0 0.05"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
<material name="framemat0"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.05 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.95 0 0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.5 0 0.95"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.5 0 0.05"/>
<geometry>
<box size="1 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="childA">
<inertial>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<mass value="1.0"/>
<inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.4 0 0.4"/>
<geometry>
<box size="0.9 0.05 0.8"/>
</geometry>
<material name="doormat0">
<color rgba="0.8 0.8 0.3 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.4 0 0.4"/>
<geometry>
<box size="0.9 0.05 0.8"/>
</geometry>
</collision>
</link>
<joint name="joint_baseLink_childA" type="continuous">
<parent link="baseLink"/>
<child link="childA"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin xyz="0.05 0 0.1"/>
<axis xyz="0 0 1"/>
</joint>
</robot>

BIN
data/slope.bullet
View File

Binary file not shown.

9
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -236,11 +236,13 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
}
}
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
jointDamping = 0.f;
jointFriction = 0.f;
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
btAssert(linkPtr);
if (linkPtr)
@ -256,6 +258,9 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
jointAxisInJointSpace = pj->m_localJointAxis;
jointLowerLimit = pj->m_lowerLimit;
jointUpperLimit = pj->m_upperLimit;
jointDamping = pj->m_jointDamping;
jointFriction = pj->m_jointFriction;
return true;
} else
{

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@ -34,7 +34,7 @@ public:
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

10
examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
View File

@ -223,18 +223,22 @@ void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3
}
}
bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
jointDamping = 0.f;
jointFriction = 0.f;
if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
{
my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
jointDamping = pj->dynamics->damping;
jointFriction = pj->dynamics->friction;
jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));

2
examples/Importers/ImportURDFDemo/ROSURDFImporter.h
View File

@ -33,7 +33,7 @@ public:
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

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

@ -195,9 +195,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btVector3 jointAxisInJointSpace;
btScalar jointLowerLimit;
btScalar jointUpperLimit;
btScalar jointDamping;
btScalar jointFriction;
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
@ -297,6 +299,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else

2
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@ -35,7 +35,7 @@ public:
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;

2
examples/SharedMemory/BodyJointInfoUtility.h
View File

@ -57,6 +57,8 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
}
info.m_jointType = mb->m_links[link].m_jointType;
info.m_jointDamping = mb->m_links[link].m_jointDamping;
info.m_jointFriction = mb->m_links[link].m_jointFriction;
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -85,6 +85,8 @@ struct b3JointInfo
int m_uIndex;
int m_jointIndex;
int m_flags;
double m_jointDamping;
double m_jointFriction;
};
struct b3JointSensorState

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

@ -1899,6 +1899,9 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
memPtr->m_linkMass = getLink(i).m_mass;
memPtr->m_parentIndex = getLink(i).m_parent;
memPtr->m_jointDamping = getLink(i).m_jointDamping;
memPtr->m_jointFriction = getLink(i).m_jointFriction;
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);

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

@ -691,8 +691,9 @@ struct btMultiBodyLinkDoubleData
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
double m_jointDamping;
double m_jointFriction;
};
@ -721,7 +722,8 @@ struct btMultiBodyLinkFloatData
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
float m_jointDamping;
float m_jointFriction;
};

8
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@ -132,8 +132,10 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
// ctor: set some sensible defaults
// ctor: set some sensible defaults
btMultibodyLink()
: m_mass(1),
m_parent(-1),
@ -146,7 +148,9 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_linkName(0),
m_jointName(0)
m_jointName(0),
m_jointDamping(0),
m_jointFriction(0)
{
m_inertiaLocal.setValue(1, 1, 1);

1568
src/LinearMath/btSerializer.cpp
View File

File diff suppressed because it is too large Load Diff