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

dirty commit: experimenting with the 6DoF grabbing/p2p constraint

Browse Source
This commit is contained in:
kubas
2014-01-09 01:03:20 +01:00
parent c0530d31ec
commit 81447aa7c5
5 changed files with 296 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2706,6 +2706,135 @@ void btMultiBody::fillContactJacobianMultiDof(int link,
}
}
void btMultiBody::fillContactJacobianMultiDof_test(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
// temporary space
int num_links = getNumLinks();
int m_dofCount = getNumDofs();
scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
scratch_m.resize(num_links + 1);
btVector3 * v_ptr = &scratch_v[0];
btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(m_dofCount);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
const btVector3 p_minus_com_world = contact_point - m_basePos;
const btVector3 &normal_lin_world = normal_lin; //convenience
const btVector3 &normal_ang_world = normal_ang;
rot_from_world[0] = btMatrix3x3(m_baseQuat);
// omega coeffients first.
btVector3 omega_coeffs_world;
omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
// then v coefficients
jac[3] = normal_lin_world[0];
jac[4] = normal_lin_world[1];
jac[5] = normal_lin_world[2];
//create link-local versions of p_minus_com and normal
p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
n_local_lin[0] = rot_from_world[0] * normal_lin_world;
n_local_ang[0] = rot_from_world[0] * normal_ang_world;
// Set remaining jac values to zero for now.
for (int i = 6; i < 6 + m_dofCount; ++i)
{
jac[i] = 0;
}
// Qdot coefficients, if necessary.
if (num_links > 0 && link > -1) {
// TODO: speed this up -- don't calculate for m_links we don't need.
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
// which is resulting in repeated work being done...)
// calculate required normals & positions in the local frames.
for (int i = 0; i < num_links; ++i) {
// transform to local frame
const int parent = m_links[i].m_parent;
const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
n_local_lin[i+1] = mtx * n_local_lin[parent+1];
n_local_ang[i+1] = mtx * n_local_ang[parent+1];
p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
// calculate the jacobian entry
switch(m_links[i].m_jointType)
{
case btMultibodyLink::eRevolute:
{
results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
break;
}
case btMultibodyLink::ePrismatic:
{
results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
break;
}
case btMultibodyLink::eSpherical:
{
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
break;
}
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case btMultibodyLink::ePlanar:
{
results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2));
break;
}
#endif
}
}
// Now copy through to output.
//printf("jac[%d] = ", link);
while (link != -1)
{
for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
{
jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
}
link = m_links[link].m_parent;
}
//printf("]\n");
}
}
void btMultiBody::fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,