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

2
setup.py
View File

@@ -419,7 +419,7 @@ else:
setup(
name = 'pybullet',
version='1.1.4',
version='1.1.6',
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

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; }