mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
dirty commit: experimenting with the 6DoF grabbing/p2p constraint
This commit is contained in:
kubas
parent
c0530d31ec
commit
81447aa7c5
@ -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,
|
||||
|
||||
@ -397,6 +397,15 @@ public:
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
|
||||
|
||||
void 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;
|
||||
|
||||
|
||||
//
|
||||
// sleeping
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "btMultiBodyConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
|
||||
|
||||
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
|
||||
:m_bodyA(bodyA),
|
||||
@ -296,16 +297,20 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
|
||||
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
solverConstraint.m_jacAindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
|
||||
#endif
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
if(multiBodyA->isMultiDof())
|
||||
multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
else
|
||||
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
#endif
|
||||
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
if(multiBodyA->isMultiDof())
|
||||
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
|
||||
@ -19,6 +19,8 @@ subject to the following restrictions:
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
|
||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
:btMultiBodyConstraint(body,0,link,-1,3,false),
|
||||
m_rigidBodyA(0),
|
||||
@ -141,3 +143,152 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
|
||||
|
||||
#include "btMultiBodyPoint2Point.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
:btMultiBodyConstraint(body,0,link,-1,6,false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB)
|
||||
{
|
||||
}
|
||||
|
||||
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
|
||||
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,6,false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
int btMultiBodyPoint2Point::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
for (int i=0;i<m_bodyA->getNumLinks();i++)
|
||||
{
|
||||
if (m_bodyA->getLink(i).m_collider)
|
||||
return m_bodyA->getLink(i).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyPoint2Point::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
|
||||
for (int i=0;i<m_bodyB->getNumLinks();i++)
|
||||
{
|
||||
col = m_bodyB->getLink(i).m_collider;
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
|
||||
// int i=1;
|
||||
for (int i=0;i<6;i++)
|
||||
{
|
||||
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
|
||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||
|
||||
|
||||
btVector3 contactNormalOnB(0,0,0);
|
||||
btVector3 normalAng(0, 0, 0);
|
||||
if(i >= 3)
|
||||
contactNormalOnB[i-3] = -1;
|
||||
else
|
||||
normalAng[i] = -1;
|
||||
|
||||
|
||||
btScalar penetration = 0;
|
||||
|
||||
// Convert local points back to world
|
||||
btVector3 pivotAworld = m_pivotInA;
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
|
||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
|
||||
} else
|
||||
{
|
||||
if (m_bodyA)
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
|
||||
} else
|
||||
{
|
||||
if (m_bodyB)
|
||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
|
||||
}
|
||||
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
|
||||
btScalar relaxation = 1.f;
|
||||
|
||||
if(i < 3)
|
||||
position = 0;
|
||||
|
||||
constraintRow.m_jacAindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+m_bodyA->getNumDofs()+6);
|
||||
btScalar* jac1=&data.m_jacobians[constraintRow.m_jacAindex];
|
||||
|
||||
m_bodyA->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
||||
|
||||
|
||||
fillMultiBodyConstraintMixed(constraintRow, data,
|
||||
contactNormalOnB,
|
||||
pivotAworld, pivotBworld,
|
||||
position,
|
||||
infoGlobal,
|
||||
relaxation,
|
||||
false);
|
||||
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
|
||||
constraintRow.m_upperLimit = m_maxAppliedImpulse;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -20,6 +20,8 @@ subject to the following restrictions:
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
|
||||
class btMultiBodyPoint2Point : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user