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

fix for b3RequestActualStateCommandComputeLinkVelocity/getLinkState link velocities for static links

Browse Source
This commit is contained in:
Erwin Coumans
2017-06-13 13:51:38 -07:00
parent ab6d5c9c53
commit d17faddff2
3 changed files with 35 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -154,6 +154,8 @@ void btMultiBody::setupFixed(int i,
m_links[i].m_mass = mass;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].setAxisTop(0, 0., 0., 0.);
m_links[i].setAxisBottom(0, btVector3(0,0,0));
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector = thisPivotToThisComOffset;
m_links[i].m_eVector = parentComToThisPivotOffset;
@@ -522,7 +524,8 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
vel[0] = quatRotate(m_baseQuat ,getBaseVel());
for (int i = 0; i < num_links; ++i) {
for (int i = 0; i < num_links; ++i)
{
const int parent = m_links[i].m_parent;
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
@@ -531,9 +534,24 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
omega[i+1], vel[i+1]);
// now add qidot * shat_i
omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
}
//only supported for revolute/prismatic joints, todo: spherical and planar joints
switch(m_links[i].m_jointType)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
btVector3 axisTop = m_links[i].getAxisTop(0);
btVector3 axisBottom = m_links[i].getAxisBottom(0);
btScalar jointVel = getJointVel(i);
omega[i+1] += jointVel * axisTop;
vel[i+1] += jointVel * axisBottom;
break;
}
default:
{
}
}
}
}
btScalar btMultiBody::getKineticEnergy() const

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

@@ -96,9 +96,18 @@ struct btMultibodyLink
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btVector3 &axis)
{
m_axes[dof].m_bottomVec = axis;
}
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_topVec.setValue(x, y, z);
}
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_bottomVec.setValue(x, y, z);
}
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }