From 96ff69276f499e3d1ccecb39626c63ddf04e0532 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:26:24 +0100 Subject: [PATCH 01/17] multidof4 patch --- .../Featherstone/btMultiBody.cpp | 2116 +++++++++++++++-- src/BulletDynamics/Featherstone/btMultiBody.h | 223 +- .../Featherstone/btMultiBodyConstraint.cpp | 110 +- .../Featherstone/btMultiBodyConstraint.h | 26 +- .../btMultiBodyConstraintSolver.cpp | 68 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 17 +- .../btMultiBodyJointLimitConstraint.cpp | 46 +- .../Featherstone/btMultiBodyJointMotor.cpp | 8 +- .../Featherstone/btMultiBodyLink.h | 467 +++- .../Featherstone/btMultiBodyLinkCollider.h | 4 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 2 +- .../btMultiBodySolverConstraint.h | 2 + 12 files changed, 2661 insertions(+), 428 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index b04bc4f95..a25a9c988 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -24,6 +24,7 @@ #include "btMultiBody.h" #include "btMultiBodyLink.h" #include "btMultiBodyLinkCollider.h" +#include "LinearMath/btTransformUtil.h" // #define INCLUDE_GYRO_TERM @@ -32,13 +33,6 @@ namespace { const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds } - - - -// -// Various spatial helper functions -// - namespace { void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates @@ -59,7 +53,7 @@ namespace { btVector3 &bottom_out) { top_out = rotation_matrix.transpose() * top_in; - bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); + bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); } btScalar SpatialDotProduct(const btVector3 &a_top, @@ -69,6 +63,17 @@ namespace { { return a_bottom.dot(b_top) + a_top.dot(b_bottom); } + + void SpatialCrossProduct(const btVector3 &a_top, + const btVector3 &a_bottom, + const btVector3 &b_top, + const btVector3 &b_bottom, + btVector3 &top_out, + btVector3 &bottom_out) + { + top_out = a_top.cross(b_top); + bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); + } } @@ -79,31 +84,67 @@ namespace { btMultiBody::btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, - bool fixed_base_, - bool can_sleep_) - : base_quat(0, 0, 0, 1), - base_mass(mass), - base_inertia(inertia), + bool fixedBase, + bool canSleep) + : m_baseQuat(0, 0, 0, 1), + m_baseMass(mass), + m_baseInertia(inertia), - fixed_base(fixed_base_), - awake(true), - can_sleep(can_sleep_), - sleep_timer(0), + m_fixedBase(fixedBase), + m_awake(true), + m_canSleep(canSleep), + m_sleepTimer(0), m_baseCollider(0), m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), m_maxAppliedImpulse(1000.f), - m_hasSelfCollision(true) + m_hasSelfCollision(true), + m_dofCount(n_links) { - links.resize(n_links); + m_links.resize(n_links); - vector_buf.resize(2*n_links); - matrix_buf.resize(n_links + 1); - m_real_buf.resize(6 + 2*n_links); - base_pos.setValue(0, 0, 0); - base_force.setValue(0, 0, 0); - base_torque.setValue(0, 0, 0); + m_vectorBuf.resize(2*n_links); + m_matrixBuf.resize(n_links + 1); + m_realBuf.resize(6 + 2*n_links); + m_basePos.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + + m_isMultiDof = false; +} + +btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass, + const btVector3 &inertia, + bool fixedBase, + bool canSleep) + : m_baseQuat(0, 0, 0, 1), + m_baseMass(mass), + m_baseInertia(inertia), + + m_fixedBase(fixedBase), + m_awake(true), + m_canSleep(canSleep), + m_sleepTimer(0), + m_baseCollider(0), + m_linearDamping(0.04f), + m_angularDamping(0.04f), + m_useGyroTerm(true), + m_maxAppliedImpulse(1000.f), + m_hasSelfCollision(true), + m_dofCount(n_dofs) +{ + m_links.resize(n_links); + + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + m_matrixBuf.resize(n_links + 1); + + m_basePos.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + + m_isMultiDof = n_links != n_dofs; } btMultiBody::~btMultiBody() @@ -114,98 +155,242 @@ void btMultiBody::setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, - const btQuaternion &rot_parent_to_this, - const btVector3 &joint_axis, - const btVector3 &r_vector_when_q_zero, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisComOffset, bool disableParentCollision) { - links[i].mass = mass; - links[i].inertia = inertia; - links[i].parent = parent; - links[i].zero_rot_parent_to_this = rot_parent_to_this; - links[i].axis_top.setValue(0,0,0); - links[i].axis_bottom = joint_axis; - links[i].e_vector = r_vector_when_q_zero; - links[i].is_revolute = false; - links[i].cached_rot_parent_to_this = rot_parent_to_this; - if (disableParentCollision) - links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + m_links[i].m_mass = mass; + m_links[i].m_inertia = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, 0., 0., 0.); + m_links[i].setAxisBottom(0, jointAxis); + m_links[i].m_eVector = parentComToThisComOffset; + m_links[i].m_cachedRotParentToThis = rotParentToThis; - links[i].updateCache(); + m_links[i].m_jointType = btMultibodyLink::ePrismatic; + m_links[i].m_dofCount = 1; + m_links[i].m_posVarCount = 1; + m_links[i].m_jointPos[0] = 0.f; + m_links[i].m_jointTorque[0] = 0.f; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + if(m_isMultiDof) + m_links[i].updateCacheMultiDof(); + else + m_links[i].updateCache(); + // + if(m_isMultiDof) + updateLinksDofOffsets(); } void btMultiBody::setupRevolute(int i, btScalar mass, const btVector3 &inertia, int parent, - const btQuaternion &zero_rot_parent_to_this, - const btVector3 &joint_axis, - const btVector3 &parent_axis_position, - const btVector3 &my_axis_position, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { - links[i].mass = mass; - links[i].inertia = inertia; - links[i].parent = parent; - links[i].zero_rot_parent_to_this = zero_rot_parent_to_this; - links[i].axis_top = joint_axis; - links[i].axis_bottom = joint_axis.cross(my_axis_position); - links[i].d_vector = my_axis_position; - links[i].e_vector = parent_axis_position; - links[i].is_revolute = true; + m_links[i].m_mass = mass; + m_links[i].m_inertia = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, jointAxis); + m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eRevolute; + m_links[i].m_dofCount = 1; + m_links[i].m_posVarCount = 1; + m_links[i].m_jointPos[0] = 0.f; + m_links[i].m_jointTorque[0] = 0.f; + if (disableParentCollision) - links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - links[i].updateCache(); + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + if(m_isMultiDof) + m_links[i].updateCacheMultiDof(); + else + m_links[i].updateCache(); + // + if(m_isMultiDof) + updateLinksDofOffsets(); } +void btMultiBody::setupSpherical(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) +{ + btAssert(m_isMultiDof); + m_links[i].m_mass = mass; + m_links[i].m_inertia = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eSpherical; + m_links[i].m_dofCount = 3; + m_links[i].m_posVarCount = 4; + m_links[i].setAxisTop(0, 1.f, 0.f, 0.f); + m_links[i].setAxisTop(1, 0.f, 1.f, 0.f); + m_links[i].setAxisTop(2, 0.f, 0.f, 1.f); + m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset)); + m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset)); + m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset)); + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f; + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; + + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} + +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS +void btMultiBody::setupPlanar(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, + bool disableParentCollision) +{ + btAssert(m_isMultiDof); + + m_links[i].m_mass = mass; + m_links[i].m_inertia = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector.setZero(); + m_links[i].m_eVector = parentComToThisComOffset; + + // + static btVector3 vecNonParallelToRotAxis(1, 0, 0); + if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) + vecNonParallelToRotAxis.setValue(0, 1, 0); + // + + m_links[i].m_jointType = btMultibodyLink::ePlanar; + m_links[i].m_dofCount = 3; + m_links[i].m_posVarCount = 3; + m_links[i].getAxisTop(0) = rotationAxis.normalized(); + m_links[i].getAxisTop(1).setZero(); + m_links[i].getAxisTop(2).setZero(); + m_links[i].getAxisBottom(0).setZero(); + m_links[i].getAxisBottom(1) = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); + m_links[i].getAxisBottom(2) = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} +#endif + +void btMultiBody::forceMultiDof() +{ + if(m_isMultiDof) return; + + m_isMultiDof = true; + + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + + updateLinksDofOffsets(); +} int btMultiBody::getParent(int i) const { - return links[i].parent; + return m_links[i].m_parent; } btScalar btMultiBody::getLinkMass(int i) const { - return links[i].mass; + return m_links[i].m_mass; } const btVector3 & btMultiBody::getLinkInertia(int i) const { - return links[i].inertia; + return m_links[i].m_inertia; } btScalar btMultiBody::getJointPos(int i) const { - return links[i].joint_pos; + return m_links[i].m_jointPos[0]; } btScalar btMultiBody::getJointVel(int i) const { - return m_real_buf[6 + i]; + return m_realBuf[6 + i]; +} + +btScalar * btMultiBody::getJointPosMultiDof(int i) +{ + return &m_links[i].m_jointPos[0]; +} + +btScalar * btMultiBody::getJointVelMultiDof(int i) +{ + return &m_realBuf[6 + m_links[i].m_dofOffset]; } void btMultiBody::setJointPos(int i, btScalar q) { - links[i].joint_pos = q; - links[i].updateCache(); + m_links[i].m_jointPos[0] = q; + m_links[i].updateCache(); +} + +void btMultiBody::setJointPosMultiDof(int i, btScalar *q) +{ + for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) + m_links[i].m_jointPos[pos] = q[pos]; + + m_links[i].updateCacheMultiDof(); } void btMultiBody::setJointVel(int i, btScalar qdot) { - m_real_buf[6 + i] = qdot; + m_realBuf[6 + i] = qdot; +} + +void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) +{ + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof]; } const btVector3 & btMultiBody::getRVector(int i) const { - return links[i].cached_r_vector; + return m_links[i].m_cachedRVector; } const btQuaternion & btMultiBody::getParentToLocalRot(int i) const { - return links[i].cached_rot_parent_to_this; + return m_links[i].m_cachedRotParentToThis; } btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const @@ -260,20 +445,20 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const { int num_links = getNumLinks(); // Calculates the velocities of each link (and the base) in its local frame - omega[0] = quatRotate(base_quat ,getBaseOmega()); - vel[0] = quatRotate(base_quat ,getBaseVel()); + omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); + vel[0] = quatRotate(m_baseQuat ,getBaseVel()); for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; + const int parent = m_links[i].m_parent; // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector, + SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, omega[parent+1], vel[parent+1], omega[i+1], vel[i+1]); // now add qidot * shat_i - omega[i+1] += getJointVel(i) * links[i].axis_top; - vel[i+1] += getJointVel(i) * links[i].axis_bottom; + omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); + vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); } } @@ -286,12 +471,12 @@ btScalar btMultiBody::getKineticEnergy() const compTreeLinkVelocities(&omega[0], &vel[0]); // we will do the factor of 0.5 at the end - btScalar result = base_mass * vel[0].dot(vel[0]); - result += omega[0].dot(base_inertia * omega[0]); + btScalar result = m_baseMass * vel[0].dot(vel[0]); + result += omega[0].dot(m_baseInertia * omega[0]); for (int i = 0; i < num_links; ++i) { - result += links[i].mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(links[i].inertia * omega[i+1]); + result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); + result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]); } return 0.5f * result; @@ -306,12 +491,12 @@ btVector3 btMultiBody::getAngularMomentum() const btAlignedObjectArray rot_from_world;rot_from_world.resize(num_links+1); compTreeLinkVelocities(&omega[0], &vel[0]); - rot_from_world[0] = base_quat; - btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0])); + rot_from_world[0] = m_baseQuat; + btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); for (int i = 0; i < num_links; ++i) { - rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1]))); + rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; + result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1]))); } return result; @@ -320,13 +505,14 @@ btVector3 btMultiBody::getAngularMomentum() const void btMultiBody::clearForcesAndTorques() { - base_force.setValue(0, 0, 0); - base_torque.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + for (int i = 0; i < getNumLinks(); ++i) { - links[i].applied_force.setValue(0, 0, 0); - links[i].applied_torque.setValue(0, 0, 0); - links[i].joint_torque = 0; + m_links[i].m_appliedForce.setValue(0, 0, 0); + m_links[i].m_appliedTorque.setValue(0, 0, 0); + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f; } } @@ -334,54 +520,69 @@ void btMultiBody::clearVelocities() { for (int i = 0; i < 6 + getNumLinks(); ++i) { - m_real_buf[i] = 0.f; + m_realBuf[i] = 0.f; } } void btMultiBody::addLinkForce(int i, const btVector3 &f) { - links[i].applied_force += f; + m_links[i].m_appliedForce += f; } void btMultiBody::addLinkTorque(int i, const btVector3 &t) { - links[i].applied_torque += t; + m_links[i].m_appliedTorque += t; } void btMultiBody::addJointTorque(int i, btScalar Q) { - links[i].joint_torque += Q; + m_links[i].m_jointTorque[0] += Q; +} + +void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) +{ + m_links[i].m_jointTorque[dof] += Q; +} + +void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) +{ + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_links[i].m_jointTorque[dof] = Q[dof]; } const btVector3 & btMultiBody::getLinkForce(int i) const { - return links[i].applied_force; + return m_links[i].m_appliedForce; } const btVector3 & btMultiBody::getLinkTorque(int i) const { - return links[i].applied_torque; + return m_links[i].m_appliedTorque; } btScalar btMultiBody::getJointTorque(int i) const { - return links[i].joint_torque; + return m_links[i].m_jointTorque[0]; } +btScalar * btMultiBody::getJointTorqueMultiDof(int i) +{ + return &m_links[i].m_jointTorque[0]; +} -inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed) +inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? { btVector3 row0 = btVector3( - v0.x() * v1Transposed.x(), - v0.x() * v1Transposed.y(), - v0.x() * v1Transposed.z()); + v0.x() * v1.x(), + v0.x() * v1.y(), + v0.x() * v1.z()); btVector3 row1 = btVector3( - v0.y() * v1Transposed.x(), - v0.y() * v1Transposed.y(), - v0.y() * v1Transposed.z()); + v0.y() * v1.x(), + v0.y() * v1.y(), + v0.y() * v1.z()); btVector3 row2 = btVector3( - v0.z() * v1Transposed.x(), - v0.z() * v1Transposed.y(), - v0.z() * v1Transposed.z()); + v0.z() * v1.x(), + v0.z() * v1.y(), + v0.z() * v1.z()); btMatrix3x3 m(row0[0],row0[1],row0[2], row1[0],row1[1],row1[2], @@ -389,6 +590,831 @@ inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Tr return m; } +#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) +// + +#ifndef TEST_SPATIAL_ALGEBRA_LAYER +void btMultiBody::stepVelocitiesMultiDof(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) +{ + // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) + // and the base linear & angular accelerations. + + // We apply damping forces in this routine as well as any external forces specified by the + // caller (via addBaseForce etc). + + // output should point to an array of 6 + num_links reals. + // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), + // num_links joint acceleration values. + + int num_links = getNumLinks(); + + const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K2_LINEAR = m_linearDamping; + + const btScalar DAMPING_K1_ANGULAR = m_angularDamping; + const btScalar DAMPING_K2_ANGULAR= m_angularDamping; + + btVector3 base_vel = getBaseVel(); + btVector3 base_omega = getBaseOmega(); + + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount + scratch_v.resize(8*num_links + 6); + scratch_m.resize(4*num_links + 4); + + btScalar * r_ptr = &scratch_r[0]; + btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results + btVector3 * v_ptr = &scratch_v[0]; + + // vhat_i (top = angular, bottom = linear part) + btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; + + // zhat_i^A + btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1; + btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1; + + // chat_i (note NOT defined for the base) + btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; + btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; + + // top left, top right and bottom left blocks of Ihat_i^A. + // bottom right block = transpose of top left block and is not stored. + // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. + btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; + btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; + btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + // hhat_i, ahat_i + // hhat is NOT stored for the base (but ahat is) + btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0; + btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0; + btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; + btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; + + // Y_i, invD_i + btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = &scratch_r[0]; + ///////////////// + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + // Start of the algorithm proper. + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + + vel_top_angular[0] = rot_from_parent[0] * base_omega; + vel_bottom_linear[0] = rot_from_parent[0] * base_vel; + + if (m_fixedBase) + { + zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0); + } + else + { + zeroAccForce[0] = - (rot_from_parent[0] * (m_baseForce + - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); + + zeroAccTorque[0] = + - (rot_from_parent[0] * m_baseTorque); + + if (m_useGyroTerm) + zeroAccTorque[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] ); + + zeroAccTorque[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); + + //NOTE: Coriolis terms are missing! (left like so following Stephen's code) + } + + inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0); + + + inertia_top_right[0].setValue(m_baseMass, 0, 0, + 0, m_baseMass, 0, + 0, 0, m_baseMass); + inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0, + 0, m_baseInertia[1], 0, + 0, 0, m_baseInertia[2]); + + rot_from_world[0] = rot_from_parent[0]; + + for (int i = 0; i < num_links; ++i) { + const int parent = m_links[i].m_parent; + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + + + rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, + vel_top_angular[parent+1], vel_bottom_linear[parent+1], + vel_top_angular[i+1], vel_bottom_linear[i+1]); + ////////////////////////////////////////////////////////////// + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + btVector3 joint_vel_spat_top, joint_vel_spat_bottom; + joint_vel_spat_top.setZero(); joint_vel_spat_bottom.setZero(); + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + joint_vel_spat_top += getJointVelMultiDof(i)[dof] * m_links[i].getAxisTop(dof); + joint_vel_spat_bottom += getJointVelMultiDof(i)[dof] * m_links[i].getAxisBottom(dof); + } + + vel_top_angular[i+1] += joint_vel_spat_top; + vel_bottom_linear[i+1] += joint_vel_spat_bottom; + + // we can now calculate chat_i + // remember vhat_i is really vhat_p(i) (but in current frame) at this point + SpatialCrossProduct(vel_top_angular[i+1], vel_bottom_linear[i+1], joint_vel_spat_top, joint_vel_spat_bottom, coriolis_top_angular[i], coriolis_bottom_linear[i]); + + // calculate zhat_i^A + // + //external forces + zeroAccForce[i+1] = -(rot_from_world[i+1] * (m_links[i].m_appliedForce)); + zeroAccTorque[i+1] = -(rot_from_world[i+1] * m_links[i].m_appliedTorque); + // + //DAMPING TERMS (ONLY) + zeroAccForce[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; + zeroAccTorque[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); + + // calculate Ihat_i^A + inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); + inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0, + 0, m_links[i].m_mass, 0, + 0, 0, m_links[i].m_mass); + inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0, + 0, m_links[i].m_inertia[1], 0, + 0, 0, m_links[i].m_inertia[2]); + + + //// + //p += v x* Iv - done in a simpler way + if(m_useGyroTerm) + zeroAccTorque[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] ); + // + coriolis_bottom_linear[i] += vel_top_angular[i+1].cross(vel_bottom_linear[i+1]) - (rot_from_parent[i+1] * (vel_top_angular[parent+1].cross(vel_bottom_linear[parent+1]))); + //coriolis_bottom_linear[i].setZero(); + + //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); + //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); + //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); + } + + static btScalar D[36]; //it's dofxdof for each body so asingle 6x6 D matrix will do + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) + { + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + //pFunMultSpatVecTimesSpatMat2(m_links[i].m_axesTop[dof], m_links[i].m_axesBottom[dof], inertia_top_left[i+1], inertia_top_right[i+1], inertia_bottom_left[i+1], h_t, h_b); + { + h_t = inertia_top_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_right[i+1] * m_links[i].getAxisBottom(dof); + h_b = inertia_bottom_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(dof); + } + + btScalar *D_row = &D[dof * m_links[i].m_dofCount]; + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + D_row[dof2] = SpatialDotProduct(m_links[i].getAxisTop(dof2), m_links[i].getAxisBottom(dof2), h_t, h_b); + } + + Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] + - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]) + - SpatialDotProduct(h_t, h_b, coriolis_top_angular[i], coriolis_bottom_linear[i]) + ; + } + + const int parent = m_links[i].m_parent; + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + invDi[0] = 1.0f / D[0]; + break; + } + case btMultibodyLink::eSpherical: +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS + case btMultibodyLink::ePlanar: +#endif + { + static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + + //unroll the loop? + for(int row = 0; row < 3; ++row) + for(int col = 0; col < 3; ++col) + invDi[row * 3 + col] = invD3x3[row][col]; + + break; + } + } + + + static btVector3 tmp_top[6]; //move to scratch mem or other buffers? (12 btVector3 will suffice) + static btVector3 tmp_bottom[6]; + + //for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + //{ + // tmp_top[dof].setZero(); + // tmp_bottom[dof].setZero(); + + // for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + // { + // btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2]; + // btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2]; + // // + // tmp_top[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_b; + // tmp_bottom[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_t; + // } + //} + + //btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1]; + + //for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + //{ + // btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + // btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + // TL -= outerProduct(h_t, tmp_top[dof]); + // TR -= outerProduct(h_t, tmp_bottom[dof]); + // BL -= outerProduct(h_b, tmp_top[dof]); + //} + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + tmp_top[dof].setZero(); + tmp_bottom[dof].setZero(); + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2]; + btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2]; + // + tmp_top[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_t; + tmp_bottom[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_b; + } + } + + btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + TL -= outerProduct(h_t, tmp_bottom[dof]); + TR -= outerProduct(h_t, tmp_top[dof]); + BL -= outerProduct(h_b, tmp_bottom[dof]); + } + + + btMatrix3x3 r_cross; + r_cross.setValue( + 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1], + m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0], + -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0); + + inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; + inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; + inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() * + (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1]; + + + btVector3 in_top, in_bottom, out_top, out_bottom; + + static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + in_top = zeroAccForce[i+1] + + inertia_top_left[i+1] * coriolis_top_angular[i] + + inertia_top_right[i+1] * coriolis_bottom_linear[i]; + + in_bottom = zeroAccTorque[i+1] + + inertia_bottom_left[i+1] * coriolis_top_angular[i] + + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]; + + //unroll the loop? + for(int row = 0; row < 3; ++row) + { + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + in_top[row] += h_t[row] * invD_times_Y[dof]; + in_bottom[row] += h_b[row] * invD_times_Y[dof]; + } + } + + InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, + in_top, in_bottom, out_top, out_bottom); + + zeroAccForce[parent+1] += out_top; + zeroAccTorque[parent+1] += out_bottom; + } + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) + { + accel_top[0] = accel_bottom[0] = btVector3(0,0,0); + } + else + { + if (num_links > 0) + { + m_cachedInertiaTopLeft = inertia_top_left[0]; + m_cachedInertiaTopRight = inertia_top_right[0]; + m_cachedInertiaLowerLeft = inertia_bottom_left[0]; + m_cachedInertiaLowerRight= inertia_top_left[0].transpose(); + + } + btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]); + btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]); + float result[6]; + + solveImatrix(rhs_top, rhs_bot, result); + for (int i = 0; i < 3; ++i) { + accel_top[0][i] = -result[i]; + accel_bottom[0][i] = -result[i+3]; + } + + } + + static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) { + const int parent = m_links[i].m_parent; + + SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, + accel_top[parent+1], accel_bottom[parent+1], + accel_top[i+1], accel_bottom[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]); + } + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + accel_top[i+1] += coriolis_top_angular[i]; + accel_bottom[i+1] += coriolis_bottom_linear[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof); + accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof); + } + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + + ///////////////// + //printf("q = ["); + //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); + //for(int link = 0; link < getNumLinks(); ++link) + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // printf("%.6f ", m_links[link].m_jointPos[dof]); + //printf("]\n"); + //// + //printf("qd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", m_realBuf[dof]); + //printf("]\n"); + //printf("qdd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", output[dof]); + //printf("]\n"); + ///////////////// + + // Final step: add the accelerations (times dt) to the velocities. + applyDeltaVeeMultiDof(output, dt); +} + +#else //i.e. TEST_SPATIAL_ALGEBRA_LAYER +void btMultiBody::stepVelocitiesMultiDof(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) +{ + // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) + // and the base linear & angular accelerations. + + // We apply damping forces in this routine as well as any external forces specified by the + // caller (via addBaseForce etc). + + // output should point to an array of 6 + num_links reals. + // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), + // num_links joint acceleration values. + + int num_links = getNumLinks(); + + const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K2_LINEAR = m_linearDamping; + + const btScalar DAMPING_K1_ANGULAR = m_angularDamping; + const btScalar DAMPING_K2_ANGULAR= m_angularDamping; + + btVector3 base_vel = getBaseVel(); + btVector3 base_omega = getBaseOmega(); + + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount + scratch_v.resize(8*num_links + 6); + scratch_m.resize(4*num_links + 4); + + btScalar * r_ptr = &scratch_r[0]; + btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results + btVector3 * v_ptr = &scratch_v[0]; + + // vhat_i (top = angular, bottom = linear part) + btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // zhat_i^A + btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // chat_i (note NOT defined for the base) + btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2; + // + // Ihat_i^A. + btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + // hhat_i, ahat_i + // hhat is NOT stored for the base (but ahat is) + btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // Y_i, invD_i + btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = &scratch_r[0]; + // + //aux variables + static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) + static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do + static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child + static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + ///////////////// + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + // Start of the algorithm proper. + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + + //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates + spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); + + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } + else + { + //external forces + zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce)); + + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 10.; + zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), + linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); + + // + //p += vhat x Ihat vhat - done in a simpler way + if (m_useGyroTerm) + zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular())); + // + zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear())); + } + + + //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) + spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), + // + btMatrix3x3(m_baseMass, 0, 0, + 0, m_baseMass, 0, + 0, 0, m_baseMass), + // + btMatrix3x3(m_baseInertia[0], 0, 0, + 0, m_baseInertia[1], 0, + 0, 0, m_baseInertia[2]) + ); + + rot_from_world[0] = rot_from_parent[0]; + + for (int i = 0; i < num_links; ++i) { + const int parent = m_links[i].m_parent; + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + ////////////////////////////////////////////////////////////// + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + spatJointVel.setZero(); + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; + // + // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint + //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; + + // we can now calculate chat_i + spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); + + // calculate zhat_i^A + // + //external forces + zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce)); + // + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 10.; + zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), + linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); + + // calculate Ihat_i^A + //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) + spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), + // + btMatrix3x3(m_links[i].m_mass, 0, 0, + 0, m_links[i].m_mass, 0, + 0, 0, m_links[i].m_mass), + // + btMatrix3x3(m_links[i].m_inertia[0], 0, 0, + 0, m_links[i].m_inertia[1], 0, + 0, 0, m_links[i].m_inertia[2]) + ); + // + //p += vhat x Ihat vhat - done in a simpler way + if(m_useGyroTerm) + zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular())); + // + zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); + + + //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); + //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); + //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); + } + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + hDof = spatInertia[i+1] * m_links[i].m_axes[dof]; + // + Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] + - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) + - spatCoriolisAcc[i].dot(hDof) + ; + } + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btScalar *D_row = &D[dof * m_links[i].m_dofCount]; + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); + } + } + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + invDi[0] = 1.0f / D[0]; + break; + } + case btMultibodyLink::eSpherical: +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS + case btMultibodyLink::ePlanar: +#endif + { + static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + + //unroll the loop? + for(int row = 0; row < 3; ++row) + { + for(int col = 0; col < 3; ++col) + { + invDi[row * 3 + col] = invD3x3[row][col]; + } + } + + break; + } + } + + //determine h*D^{-1} + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + spatForceVecTemps[dof].setZero(); + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + // + spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; + } + } + + dyadTemp = spatInertia[i+1]; + + //determine (h*D^{-1}) * h^{T} + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); + } + + fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + } + + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); + + zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; + } + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) + { + spatAcc[0].setZero(); + } + else + { + if (num_links > 0) + { + m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat; + m_cachedInertiaTopRight = spatInertia[0].m_topRightMat; + m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat; + m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose(); + + } + + solveImatrix(zeroAccSpatFrc[0], result); + spatAcc[0] = -result; + } + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + } + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + spatAcc[i+1] += spatCoriolisAcc[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + + ///////////////// + //printf("q = ["); + //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); + //for(int link = 0; link < getNumLinks(); ++link) + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // printf("%.6f ", m_links[link].m_jointPos[dof]); + //printf("]\n"); + //// + //printf("qd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", m_realBuf[dof]); + //printf("]\n"); + //printf("qdd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", output[dof]); + //printf("]\n"); + ///////////////// + + // Final step: add the accelerations (times dt) to the velocities. + applyDeltaVeeMultiDof(output, dt); +} +#endif + void btMultiBody::stepVelocities(btScalar dt, btAlignedObjectArray &scratch_r, @@ -447,19 +1473,19 @@ void btMultiBody::stepVelocities(btScalar dt, btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3 * rot_from_parent = &matrix_buf[0]; + btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; btMatrix3x3 * rot_from_world = &scratch_m[0]; // hhat_i, ahat_i // hhat is NOT stored for the base (but ahat is) - btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; - btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; + btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0; + btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0; btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; // Y_i, D_i btScalar * Y = r_ptr; r_ptr += num_links; - btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; + btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0; // ptr to the joint accel part of the output btScalar * joint_accel = output + 6; @@ -470,24 +1496,24 @@ void btMultiBody::stepVelocities(btScalar dt, // First 'upward' loop. // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - rot_from_parent[0] = btMatrix3x3(base_quat); + rot_from_parent[0] = btMatrix3x3(m_baseQuat); vel_top_angular[0] = rot_from_parent[0] * base_omega; vel_bottom_linear[0] = rot_from_parent[0] * base_vel; - if (fixed_base) { + if (m_fixedBase) { zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); } else { - zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force - - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); + zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce + - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); zero_acc_bottom_linear[0] = - - (rot_from_parent[0] * base_torque); + - (rot_from_parent[0] * m_baseTorque); if (m_useGyroTerm) - zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] ); + zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] ); - zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); + zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); } @@ -496,64 +1522,64 @@ void btMultiBody::stepVelocities(btScalar dt, inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - inertia_top_right[0].setValue(base_mass, 0, 0, - 0, base_mass, 0, - 0, 0, base_mass); - inertia_bottom_left[0].setValue(base_inertia[0], 0, 0, - 0, base_inertia[1], 0, - 0, 0, base_inertia[2]); + inertia_top_right[0].setValue(m_baseMass, 0, 0, + 0, m_baseMass, 0, + 0, 0, m_baseMass); + inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0, + 0, m_baseInertia[1], 0, + 0, 0, m_baseInertia[2]); rot_from_world[0] = rot_from_parent[0]; for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this); + const int parent = m_links[i].m_parent; + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; // vhat_i = i_xhat_p(i) * vhat_p(i) - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, vel_top_angular[parent+1], vel_bottom_linear[parent+1], vel_top_angular[i+1], vel_bottom_linear[i+1]); // we can now calculate chat_i // remember vhat_i is really vhat_p(i) (but in current frame) at this point - coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector)) - + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i); - if (links[i].is_revolute) { - coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i); - coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom); + coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector)) + + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i); + if (m_links[i].m_jointType == btMultibodyLink::eRevolute) { + coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i); + coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0)); } else { coriolis_top_angular[i] = btVector3(0,0,0); } // now set vhat_i to its true value by doing // vhat_i += qidot * shat_i - vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top; - vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom; + vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); + vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); // calculate zhat_i^A - zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force)); - zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; + zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce)); + zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; zero_acc_bottom_linear[i+1] = - - (rot_from_world[i+1] * links[i].applied_torque); + - (rot_from_world[i+1] * m_links[i].m_appliedTorque); if (m_useGyroTerm) { - zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] ); + zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] ); } - zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); + zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); // calculate Ihat_i^A inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - inertia_top_right[i+1].setValue(links[i].mass, 0, 0, - 0, links[i].mass, 0, - 0, 0, links[i].mass); - inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0, - 0, links[i].inertia[1], 0, - 0, 0, links[i].inertia[2]); + inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0, + 0, m_links[i].m_mass, 0, + 0, 0, m_links[i].m_mass); + inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0, + 0, m_links[i].m_inertia[1], 0, + 0, 0, m_links[i].m_inertia[2]); } @@ -561,15 +1587,15 @@ void btMultiBody::stepVelocities(btScalar dt, // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { - h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom; - h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom; - btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]); + h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0); + h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0); + btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]); D[i] = val; - Y[i] = links[i].joint_torque - - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) + Y[i] = m_links[i].m_jointTorque[0] + - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); - const int parent = links[i].parent; + const int parent = m_links[i].m_parent; // Ip += pXi * (Ii - hi hi' / Di) * iXp @@ -585,9 +1611,9 @@ void btMultiBody::stepVelocities(btScalar dt, btMatrix3x3 r_cross; r_cross.setValue( - 0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1], - links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0], - -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0); + 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1], + m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0], + -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0); inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; @@ -606,7 +1632,7 @@ void btMultiBody::stepVelocities(btScalar dt, + inertia_bottom_left[i+1] * coriolis_top_angular[i] + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i] + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, in_top, in_bottom, out_top, out_bottom); zero_acc_top_angular[parent+1] += out_top; zero_acc_bottom_linear[parent+1] += out_bottom; @@ -616,7 +1642,7 @@ void btMultiBody::stepVelocities(btScalar dt, // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (fixed_base) + if (m_fixedBase) { accel_top[0] = accel_bottom[0] = btVector3(0,0,0); } @@ -631,10 +1657,10 @@ void btMultiBody::stepVelocities(btScalar dt, //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose(); //cached_imatrix_lu.reset(new Eigen::LU >(Imatrix)); // TODO: Avoid memory allocation here? - cached_inertia_top_left = inertia_top_left[0]; - cached_inertia_top_right = inertia_top_right[0]; - cached_inertia_lower_left = inertia_bottom_left[0]; - cached_inertia_lower_right= inertia_top_left[0].transpose(); + m_cachedInertiaTopLeft = inertia_top_left[0]; + m_cachedInertiaTopRight = inertia_top_right[0]; + m_cachedInertiaLowerLeft = inertia_bottom_left[0]; + m_cachedInertiaLowerRight= inertia_top_left[0].transpose(); } btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); @@ -650,15 +1676,15 @@ void btMultiBody::stepVelocities(btScalar dt, } - // now do the loop over the links + // now do the loop over the m_links for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + const int parent = m_links[i].m_parent; + SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, accel_top[parent+1], accel_bottom[parent+1], accel_top[i+1], accel_bottom[i+1]); joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom; + accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0); + accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0); } // transform base accelerations back to the world frame. @@ -671,38 +1697,55 @@ void btMultiBody::stepVelocities(btScalar dt, output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; + + ///////////////// + //printf("q = ["); + //printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); + //for(int link = 0; link < getNumLinks(); ++link) + // printf("%.2f ", m_links[link].m_jointPos[0]); + //printf("]\n"); + //// + //printf("qd = ["); + //for(int dof = 0; dof < getNumLinks() + 6; ++dof) + // printf("%.2f ", m_realBuf[dof]); + //printf("]\n"); + //// + //printf("qdd = ["); + //for(int dof = 0; dof < getNumLinks() + 6; ++dof) + // printf("%.2f ", output[dof]); + //printf("]\n"); + ///////////////// + // Final step: add the accelerations (times dt) to the velocities. applyDeltaVee(output, dt); } - - void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const { int num_links = getNumLinks(); ///solve I * x = rhs, so the result = invI * rhs if (num_links == 0) { - // in the case of 0 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result[0] = rhs_bot[0] / base_inertia[0]; - result[1] = rhs_bot[1] / base_inertia[1]; - result[2] = rhs_bot[2] / base_inertia[2]; - result[3] = rhs_top[0] / base_mass; - result[4] = rhs_top[1] / base_mass; - result[5] = rhs_top[2] / base_mass; + // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + result[0] = rhs_bot[0] / m_baseInertia[0]; + result[1] = rhs_bot[1] / m_baseInertia[1]; + result[2] = rhs_bot[2] / m_baseInertia[2]; + result[3] = rhs_top[0] / m_baseMass; + result[4] = rhs_top[1] / m_baseMass; + result[5] = rhs_top[2] / m_baseMass; } else { /// Special routine for calculating the inverse of a spatial inertia matrix ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f; - btMatrix3x3 tmp = cached_inertia_lower_right * Binv; - btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse(); - tmp = invIupper_right * cached_inertia_lower_right; + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; + btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); + tmp = invIupper_right * m_cachedInertiaLowerRight; btMatrix3x3 invI_upper_left = (tmp * Binv); btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = cached_inertia_top_left * invI_upper_left; + tmp = m_cachedInertiaTopLeft * invI_upper_left; tmp[0][0]-= 1.0; tmp[1][1]-= 1.0; tmp[2][2]-= 1.0; @@ -727,6 +1770,406 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo } } +#ifdef TEST_SPATIAL_ALGEBRA_LAYER +void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const +{ + int num_links = getNumLinks(); + ///solve I * x = rhs, so the result = invI * rhs + if (num_links == 0) + { + // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + result.setAngular(rhs.getAngular() / m_baseInertia); + result.setLinear(rhs.getLinear() / m_baseMass); + } else + { + /// Special routine for calculating the inverse of a spatial inertia matrix + ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; + btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); + tmp = invIupper_right * m_cachedInertiaLowerRight; + btMatrix3x3 invI_upper_left = (tmp * Binv); + btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); + tmp = m_cachedInertiaTopLeft * invI_upper_left; + tmp[0][0]-= 1.0; + tmp[1][1]-= 1.0; + tmp[2][2]-= 1.0; + btMatrix3x3 invI_lower_left = (Binv * tmp); + + //multiply result = invI * rhs + { + btVector3 vtop = invI_upper_left*rhs.getLinear(); + btVector3 tmp; + tmp = invIupper_right * rhs.getAngular(); + vtop += tmp; + btVector3 vbot = invI_lower_left*rhs.getLinear(); + tmp = invI_lower_right * rhs.getAngular(); + vbot += tmp; + result.setVector(vtop, vbot); + } + + } +} +#endif + +void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const +{ + for (int row = 0; row < rowsA; row++) + { + for (int col = 0; col < colsB; col++) + { + pC[row * colsB + col] = 0.f; + for (int inner = 0; inner < rowsB; inner++) + { + pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB]; + } + } + } +} + +#ifndef TEST_SPATIAL_ALGEBRA_LAYER +void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const +{ + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + int num_links = getNumLinks(); + int m_dofCount = getNumDofs(); + scratch_r.resize(m_dofCount); + scratch_v.resize(4*num_links + 4); + + btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; + btVector3 * v_ptr = &scratch_v[0]; + + // zhat_i^A (scratch space) + btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1; + btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1; + + // rot_from_parent (cached from calcAccelerations) + const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + + // hhat (cached), accel (scratch) + // hhat is NOT stored for the base (but ahat is) + const btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0; + const btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0; + btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; + btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; + + // Y_i (scratch), invD_i (cached) + const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = r_ptr; + //////////////// + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + btVector3 input_force(force[3],force[4],force[5]); + btVector3 input_torque(force[0],force[1],force[2]); + + // Fill in zero_acc + // -- set to force/torque on the base, zero otherwise + if (m_fixedBase) + { + zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0); + } else + { + zeroAccForce[0] = - (rot_from_parent[0] * input_force); + zeroAccTorque[0] = - (rot_from_parent[0] * input_torque); + } + for (int i = 0; i < num_links; ++i) + { + zeroAccForce[i+1] = zeroAccTorque[i+1] = btVector3(0,0,0); + } + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) + { + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]); + + Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] + - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]) + ; + } + + btScalar aa = Y[i]; + const int parent = m_links[i].m_parent; + + btVector3 in_top, in_bottom, out_top, out_bottom; + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + + static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + // Zp += pXi * (Zi + hi*Yi/Di) + in_top = zeroAccForce[i+1]; + in_bottom = zeroAccTorque[i+1]; + + //unroll the loop? + for(int row = 0; row < 3; ++row) + { + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + in_top[row] += h_t[row] * invD_times_Y[dof]; + in_bottom[row] += h_b[row] * invD_times_Y[dof]; + } + } + + InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, + in_top, in_bottom, out_top, out_bottom); + zeroAccForce[parent+1] += out_top; + zeroAccTorque[parent+1] += out_bottom; + } + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) + { + accel_top[0] = accel_bottom[0] = btVector3(0,0,0); + } + else + { + btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]); + btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]); + float result[6]; + + solveImatrix(rhs_top, rhs_bot, result); + for (int i = 0; i < 3; ++i) { + accel_top[0][i] = -result[i]; + accel_bottom[0][i] = -result[i+3]; + } + + } + + static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) { + const int parent = m_links[i].m_parent; + + SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, + accel_top[parent+1], accel_bottom[parent+1], + accel_top[i+1], accel_bottom[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof]; + const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof]; + + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]); + } + + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + mulMatrix(const_cast(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof); + accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof); + } + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out; + omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out; + vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; + + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + + ///////////////// + //printf("delta = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.2f ", output[dof]); + //printf("]\n"); + ///////////////// +} +#else //i.e. TEST_SPATIAL_ALGEBRA_LAYER +void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const +{ + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + int num_links = getNumLinks(); + scratch_r.resize(m_dofCount); + scratch_v.resize(4*num_links + 4); + + btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; + btVector3 * v_ptr = &scratch_v[0]; + + // zhat_i^A (scratch space) + btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + v_ptr += num_links * 2 + 2; + + // rot_from_parent (cached from calcAccelerations) + const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + + // hhat (cached), accel (scratch) + // hhat is NOT stored for the base (but ahat is) + const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + + // Y_i (scratch), invD_i (cached) + const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = r_ptr; + //////////////// + //aux variables + static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + static btSpatialTransformationMatrix fromParent; + ///////////////// + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + // Fill in zero_acc + // -- set to force/torque on the base, zero otherwise + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } else + { + //test forces + fromParent.m_rotMat = rot_from_parent[0]; + fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]); + } + for (int i = 0; i < num_links; ++i) + { + zeroAccSpatFrc[i+1].setZero(); + } + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] + - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) + ; + } + + btVector3 in_top, in_bottom, out_top, out_bottom; + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + // Zp += pXi * (Zi + hi*Yi/Di) + spatForceVecTemps[0] = zeroAccSpatFrc[i+1]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + } + + + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); + + zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; + } + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) + { + spatAcc[0].setZero(); + } + else + { + solveImatrix(zeroAccSpatFrc[0], result); + spatAcc[0] = -result; + + } + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + } + + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + mulMatrix(const_cast(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out; + omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out; + vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + + ///////////////// + //printf("delta = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.2f ", output[dof]); + //printf("]\n"); + ///////////////// +} +#endif void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output, @@ -746,17 +2189,17 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; // rot_from_parent (cached from calcAccelerations) - const btMatrix3x3 * rot_from_parent = &matrix_buf[0]; + const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; // hhat (cached), accel (scratch) - const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; - const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; + const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0; + const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0; btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; // Y_i (scratch), D_i (cached) btScalar * Y = r_ptr; r_ptr += num_links; - const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; + const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0; btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); @@ -771,7 +2214,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // Fill in zero_acc // -- set to force/torque on the base, zero otherwise - if (fixed_base) + if (m_fixedBase) { zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); } else @@ -787,18 +2230,18 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // 'Downward' loop. for (int i = num_links - 1; i >= 0; --i) { - - Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); + btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); + Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); Y[i] += force[6 + i]; // add joint torque - const int parent = links[i].parent; + const int parent = m_links[i].m_parent; // Zp += pXi * (Zi + hi*Yi/Di) btVector3 in_top, in_bottom, out_top, out_bottom; const btScalar Y_over_D = Y[i] / D[i]; in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i]; in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, in_top, in_bottom, out_top, out_bottom); zero_acc_top_angular[parent+1] += out_top; zero_acc_bottom_linear[parent+1] += out_bottom; @@ -808,7 +2251,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output btScalar * joint_accel = output + 6; // Second 'upward' loop - if (fixed_base) + if (m_fixedBase) { accel_top[0] = accel_bottom[0] = btVector3(0,0,0); } else @@ -827,15 +2270,16 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output } - // now do the loop over the links + // now do the loop over the m_links for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + const int parent = m_links[i].m_parent; + SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, accel_top[parent+1], accel_bottom[parent+1], accel_top[i+1], accel_bottom[i+1]); - joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom; + btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])); + joint_accel[i] = Y_minus_hT_a / D[i]; + accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0); + accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0); } // transform base accelerations back to the world frame. @@ -851,6 +2295,15 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; + + ///////////////// + //printf("delta = ["); + //for(int dof = 0; dof < getNumLinks() + 6; ++dof) + // printf("%.2f ", output[dof]); + //printf("]\n"); + ///////////////// + + int dummy = 0; } void btMultiBody::stepPositions(btScalar dt) @@ -858,7 +2311,7 @@ void btMultiBody::stepPositions(btScalar dt) int num_links = getNumLinks(); // step position by adding dt * velocity btVector3 v = getBaseVel(); - base_pos += dt * v; + m_basePos += dt * v; // "exponential map" method for the rotation btVector3 base_omega = getBaseOmega(); @@ -870,22 +2323,235 @@ void btMultiBody::stepPositions(btScalar dt) const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2 const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega| const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|) - base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); + m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); } else { - base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt); + m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt); } // Make sure the quaternion represents a valid rotation. // (Not strictly necessary, but helps prevent any round-off errors from building up.) - base_quat.normalize(); + m_baseQuat.normalize(); - // Finally we can update joint_pos for each of the links + // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { float jointVel = getJointVel(i); - links[i].joint_pos += dt * jointVel; - links[i].updateCache(); + m_links[i].m_jointPos[0] += dt * jointVel; + m_links[i].updateCache(); + } +} + +void btMultiBody::stepPositionsMultiDof(btScalar dt) +{ + int num_links = getNumLinks(); + // step position by adding dt * velocity + btVector3 v = getBaseVel(); + m_basePos += dt * v; + + /////////////////////////////// + //local functor for quaternion integration (to avoid error prone redundancy) + struct + { + //"exponential map" based on btTransformUtil::integrateTransform(..) + void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + { + //baseBody => quat is alias and omega is global coor + //!baseBody => quat is alibi and omega is local coor + + btVector3 axis; + btVector3 angvel; + + if(!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + else + angvel = omega; + + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) + { + fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; + } + + if ( fAngle < btScalar(0.001) ) + { + // use Taylor's expansions of sync function + axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); + } + + if(!baseBody) + quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; + else + quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + + quat.normalize(); + } + } pQuatUpdateFun; + /////////////////////////////// + + pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + + // Finally we can update m_jointPos for each of the m_links + for (int i = 0; i < num_links; ++i) + { + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + btScalar jointVel = getJointVelMultiDof(i)[0]; + m_links[i].m_jointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + btScalar *pJointVel = getJointVelMultiDof(i); + static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + static btQuaternion jointOri; jointOri.setValue(m_links[i].m_jointPos[0], m_links[i].m_jointPos[1], m_links[i].m_jointPos[2], m_links[i].m_jointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + m_links[i].m_jointPos[0] = jointOri.x(); m_links[i].m_jointPos[1] = jointOri.y(); m_links[i].m_jointPos[2] = jointOri.z(); m_links[i].m_jointPos[3] = jointOri.w(); + break; + } +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS + case btMultibodyLink::ePlanar: + { + m_links[i].m_jointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), m_links[i].m_jointPos[0]), q0_coors_qd1qd2); + m_links[i].m_jointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + m_links[i].m_jointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + + break; + } +#endif + } + + m_links[i].updateCacheMultiDof(); + } +} + +void btMultiBody::fillContactJacobianMultiDof(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const +{ + // temporary space + int num_links = getNumLinks(); + int m_dofCount = getNumDofs(); + scratch_v.resize(2*num_links + 2); + scratch_m.resize(num_links + 1); + + btVector3 * v_ptr = &scratch_v[0]; + btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local = 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; + + rot_from_world[0] = btMatrix3x3(m_baseQuat); + + p_minus_com[0] = rot_from_world[0] * p_minus_com_world; + n_local[0] = rot_from_world[0] * normal; + + // omega coeffients first. + btVector3 omega_coeffs; + omega_coeffs = p_minus_com_world.cross(normal); + jac[0] = omega_coeffs[0]; + jac[1] = omega_coeffs[1]; + jac[2] = omega_coeffs[2]; + // then v coefficients + jac[3] = normal[0]; + jac[4] = normal[1]; + jac[5] = normal[2]; + + // 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[i+1] = mtx * n_local[parent+1]; + p_minus_com[i+1] = mtx * p_minus_com[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[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0)); + break; + } + case btMultibodyLink::ePrismatic: + { + results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisBottom(0)); + break; + } + case btMultibodyLink::eSpherical: + { + results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(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[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"); } } @@ -912,9 +2578,9 @@ void btMultiBody::fillContactJacobian(int link, btMatrix3x3 * rot_from_world = &scratch_m[0]; - const btVector3 p_minus_com_world = contact_point - base_pos; + const btVector3 p_minus_com_world = contact_point - m_basePos; - rot_from_world[0] = btMatrix3x3(base_quat); + rot_from_world[0] = btMatrix3x3(m_baseQuat); p_minus_com[0] = rot_from_world[0] * p_minus_com_world; n_local[0] = rot_from_world[0] * normal; @@ -938,7 +2604,7 @@ void btMultiBody::fillContactJacobian(int link, // Qdot coefficients, if necessary. if (num_links > 0 && link > -1) { - // TODO: speed this up -- don't calculate for links we don't need. + // 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...) @@ -946,64 +2612,76 @@ void btMultiBody::fillContactJacobian(int link, for (int i = 0; i < num_links; ++i) { // transform to local frame - const int parent = links[i].parent; - const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this); + 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[i+1] = mtx * n_local[parent+1]; - p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector; + p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector; // calculate the jacobian entry - if (links[i].is_revolute) { - results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom ); + if (m_links[i].m_jointType == btMultibodyLink::eRevolute) { + results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) ); } else { - results[i] = n_local[i+1].dot( links[i].axis_bottom ); + results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) ); } } // Now copy through to output. - while (link != -1) { + //printf("jac[%d] = ", link); + while (link != -1) + { jac[6 + link] = results[link]; - link = links[link].parent; + //printf("%.2f\t", jac[6 + link]); + link = m_links[link].m_parent; } + //printf("]\n"); } } void btMultiBody::wakeUp() { - awake = true; + m_awake = true; } void btMultiBody::goToSleep() { - awake = false; + m_awake = false; } void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) { int num_links = getNumLinks(); extern bool gDisableDeactivation; - if (!can_sleep || gDisableDeactivation) + if (!m_canSleep || gDisableDeactivation) { - awake = true; - sleep_timer = 0; + m_awake = true; + m_sleepTimer = 0; return; } // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) btScalar motion = 0; - for (int i = 0; i < 6 + num_links; ++i) { - motion += m_real_buf[i] * m_real_buf[i]; - } + if(m_isMultiDof) + { + for (int i = 0; i < 6 + m_dofCount; ++i) + motion += m_realBuf[i] * m_realBuf[i]; + } + else + { + for (int i = 0; i < 6 + num_links; ++i) + motion += m_realBuf[i] * m_realBuf[i]; + } + if (motion < SLEEP_EPSILON) { - sleep_timer += timestep; - if (sleep_timer > SLEEP_TIMEOUT) { + m_sleepTimer += timestep; + if (m_sleepTimer > SLEEP_TIMEOUT) { goToSleep(); } } else { - sleep_timer = 0; - if (!awake) + m_sleepTimer = 0; + if (!m_awake) wakeUp(); } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 9f2e99abd..048e3341c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -45,21 +45,28 @@ public: // initialization // - btMultiBody(int n_links, // NOT including the base + btMultiBody(int n_links, // NOT including the base btScalar mass, // mass of base const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal - bool fixed_base_, // whether the base is fixed (true) or can move (false) - bool can_sleep_); + bool fixedBase, // whether the base is fixed (true) or can move (false) + bool canSleep); - ~btMultiBody(); + btMultiBody(int n_links, // NOT including the base + int n_dofs, // NOT including 6 floating base dofs + btScalar mass, // mass of base + const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal + bool fixedBase, // whether the base is fixed (true) or can move (false) + bool canSleep); + + ~btMultiBody(); void setupPrismatic(int i, // 0 to num_links-1 btScalar mass, const btVector3 &inertia, // in my frame; assumed diagonal int parent, - const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame. - const btVector3 &joint_axis, // in my frame - const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0. + const btQuaternion &rotParentToThis, // rotate points in parent frame to my frame. + const btVector3 &jointAxis, // in my frame + const btVector3 &parentComToThisComOffset, // vector from parent COM to my COM, in my frame, when q = 0. bool disableParentCollision=false ); @@ -67,20 +74,40 @@ public: btScalar mass, const btVector3 &inertia, int parent, - const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &joint_axis, // in my frame - const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &jointAxis, // in my frame + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame bool disableParentCollision=false); + + void setupSpherical(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision=false); + +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS + void setupPlanar(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame + bool disableParentCollision=false); +#endif const btMultibodyLink& getLink(int index) const { - return links[index]; + return m_links[index]; } btMultibodyLink& getLink(int index) { - return links[index]; + return m_links[index]; } @@ -106,12 +133,13 @@ public: // - // get number of links, masses, moments of inertia + // get number of m_links, masses, moments of inertia // - int getNumLinks() const { return links.size(); } - btScalar getBaseMass() const { return base_mass; } - const btVector3 & getBaseInertia() const { return base_inertia; } + int getNumLinks() const { return m_links.size(); } + int getNumDofs() const { return m_dofCount; } + btScalar getBaseMass() const { return m_baseMass; } + const btVector3 & getBaseInertia() const { return m_baseInertia; } btScalar getLinkMass(int i) const; const btVector3 & getLinkInertia(int i) const; @@ -120,55 +148,60 @@ public: // change mass (incomplete: can only change base mass and inertia at present) // - void setBaseMass(btScalar mass) { base_mass = mass; } - void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; } + void setBaseMass(btScalar mass) { m_baseMass = mass; } + void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } // // get/set pos/vel/rot/omega for the base link // - const btVector3 & getBasePos() const { return base_pos; } // in world frame + const btVector3 & getBasePos() const { return m_basePos; } // in world frame const btVector3 getBaseVel() const { - return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); + return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); } // in world frame const btQuaternion & getWorldToBaseRot() const { - return base_quat; + return m_baseQuat; } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame + btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame void setBasePos(const btVector3 &pos) { - base_pos = pos; + m_basePos = pos; } void setBaseVel(const btVector3 &vel) { - m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; + m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; } void setWorldToBaseRot(const btQuaternion &rot) { - base_quat = rot; + m_baseQuat = rot; //m_baseQuat asumed to ba alias!? } void setBaseOmega(const btVector3 &omega) { - m_real_buf[0]=omega[0]; - m_real_buf[1]=omega[1]; - m_real_buf[2]=omega[2]; + m_realBuf[0]=omega[0]; + m_realBuf[1]=omega[1]; + m_realBuf[2]=omega[2]; } // - // get/set pos/vel for child links (i = 0 to num_links-1) + // get/set pos/vel for child m_links (i = 0 to num_links-1) // btScalar getJointPos(int i) const; btScalar getJointVel(int i) const; + btScalar * getJointVelMultiDof(int i); + btScalar * getJointPosMultiDof(int i); + void setJointPos(int i, btScalar q); void setJointVel(int i, btScalar qdot); + void setJointPosMultiDof(int i, btScalar *q); + void setJointVelMultiDof(int i, btScalar *qdot); // // direct access to velocities as a vector of 6 + num_links elements. @@ -176,7 +209,7 @@ public: // const btScalar * getVelocityVector() const { - return &m_real_buf[0]; + return &m_realBuf[0]; } /* btScalar * getVelocityVector() { @@ -185,7 +218,7 @@ public: */ // - // get the frames of reference (positions and orientations) of the child links + // get the frames of reference (positions and orientations) of the child m_links // (i = 0 to num_links-1) // @@ -220,18 +253,21 @@ public: void addBaseForce(const btVector3 &f) { - base_force += f; + m_baseForce += f; } - void addBaseTorque(const btVector3 &t) { base_torque += t; } + void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } void addLinkForce(int i, const btVector3 &f); void addLinkTorque(int i, const btVector3 &t); void addJointTorque(int i, btScalar Q); + void addJointTorqueMultiDof(int i, int dof, btScalar Q); + void addJointTorqueMultiDof(int i, const btScalar *Q); - const btVector3 & getBaseForce() const { return base_force; } - const btVector3 & getBaseTorque() const { return base_torque; } + const btVector3 & getBaseForce() const { return m_baseForce; } + const btVector3 & getBaseTorque() const { return m_baseTorque; } const btVector3 & getLinkForce(int i) const; const btVector3 & getLinkTorque(int i) const; btScalar getJointTorque(int i) const; + btScalar * getJointTorqueMultiDof(int i); // @@ -255,6 +291,11 @@ public: btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m); + void stepVelocitiesMultiDof(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m); + // calcAccelerationDeltas // input: force vector (in same format as jacobian, i.e.: // 3 torque values, 3 force values, num_links joint torque values) @@ -265,13 +306,17 @@ public: btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const; + void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v) const; + // apply a delta-vee directly. used in sequential impulses code. void applyDeltaVee(const btScalar * delta_vee) { for (int i = 0; i < 6 + getNumLinks(); ++i) { - m_real_buf[i] += delta_vee[i]; + m_realBuf[i] += delta_vee[i]; } } @@ -300,12 +345,37 @@ public: for (int i = 0; i < 6 + getNumLinks(); ++i) { sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; - m_real_buf[i] += delta_vee[i] * multiplier; + m_realBuf[i] += delta_vee[i] * multiplier; + } + } + + void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) + { + //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + // printf("%.4f ", delta_vee[dof]*multiplier); + //printf("\n"); + + btScalar sum = 0; + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; + } + btScalar l = btSqrt(sum); + + if (l>m_maxAppliedImpulse) + { + multiplier *= m_maxAppliedImpulse/l; + } + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_realBuf[dof] += delta_vee[dof] * multiplier; } } // timestep the positions (given current velocities). void stepPositions(btScalar dt); + void stepPositionsMultiDof(btScalar dt); // @@ -323,23 +393,31 @@ public: btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m) const; + void fillContactJacobianMultiDof(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const; + // // sleeping // void setCanSleep(bool canSleep) { - can_sleep = canSleep; + m_canSleep = canSleep; } - bool isAwake() const { return awake; } + bool isAwake() const { return m_awake; } void wakeUp(); void goToSleep(); void checkMotionAndSleepIfRequired(btScalar timestep); bool hasFixedBase() const { - return fixed_base; + return m_fixedBase; } int getCompanionId() const @@ -352,9 +430,9 @@ public: m_companionId = id; } - void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links + void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links { - links.resize(numLinks); + m_links.resize(numLinks); } btScalar getLinearDamping() const @@ -369,6 +447,10 @@ public: { return m_angularDamping; } + void setAngularDamping( btScalar damp) + { + m_angularDamping = damp; + } bool getUseGyroTerm() const { @@ -396,6 +478,9 @@ public: return m_hasSelfCollision; } + bool isMultiDof() { return m_isMultiDof; } + void forceMultiDof(); + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -403,57 +488,64 @@ private: void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; - +#ifdef TEST_SPATIAL_ALGEBRA_LAYER + void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; +#endif + + void updateLinksDofOffsets() { int dofOffset = 0; for(int bidx = 0; bidx < m_links.size(); ++bidx) { m_links[bidx].m_dofOffset = dofOffset; dofOffset += m_links[bidx].m_dofCount; } } + + void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; private: btMultiBodyLinkCollider* m_baseCollider;//can be NULL - btVector3 base_pos; // position of COM of base (world frame) - btQuaternion base_quat; // rotates world points into base frame + btVector3 m_basePos; // position of COM of base (world frame) + btQuaternion m_baseQuat; // rotates world points into base frame - btScalar base_mass; // mass of the base - btVector3 base_inertia; // inertia of the base (in local frame; diagonal) + btScalar m_baseMass; // mass of the base + btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) - btVector3 base_force; // external force applied to base. World frame. - btVector3 base_torque; // external torque applied to base. World frame. + btVector3 m_baseForce; // external force applied to base. World frame. + btVector3 m_baseTorque; // external torque applied to base. World frame. - btAlignedObjectArray links; // array of links, excluding the base. index from 0 to num_links-1. + btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; + int m_dofCount; // - // real_buf: + // realBuf: // offset size array // 0 6 + num_links v (base_omega; base_vel; joint_vels) // 6+num_links num_links D // - // vector_buf: + // vectorBuf: // offset size array // 0 num_links h_top // num_links num_links h_bottom // - // matrix_buf: + // matrixBuf: // offset size array // 0 num_links+1 rot_from_parent // - btAlignedObjectArray m_real_buf; - btAlignedObjectArray vector_buf; - btAlignedObjectArray matrix_buf; + btAlignedObjectArray m_realBuf; + btAlignedObjectArray m_vectorBuf; + btAlignedObjectArray m_matrixBuf; //std::auto_ptr > > cached_imatrix_lu; - btMatrix3x3 cached_inertia_top_left; - btMatrix3x3 cached_inertia_top_right; - btMatrix3x3 cached_inertia_lower_left; - btMatrix3x3 cached_inertia_lower_right; + btMatrix3x3 m_cachedInertiaTopLeft; + btMatrix3x3 m_cachedInertiaTopRight; + btMatrix3x3 m_cachedInertiaLowerLeft; + btMatrix3x3 m_cachedInertiaLowerRight; - bool fixed_base; + bool m_fixedBase; // Sleep parameters. - bool awake; - bool can_sleep; - btScalar sleep_timer; + bool m_awake; + bool m_canSleep; + btScalar m_sleepTimer; int m_companionId; btScalar m_linearDamping; @@ -461,6 +553,7 @@ private: bool m_useGyroTerm; btScalar m_maxAppliedImpulse; bool m_hasSelfCollision; + bool m_isMultiDof; }; #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 8053ee770..1439755be 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -6,14 +6,31 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod m_bodyB(bodyB), m_linkA(linkA), m_linkB(linkB), - m_num_rows(numRows), + m_numRows(numRows), m_isUnilateral(isUnilateral), - m_maxAppliedImpulse(100) + m_maxAppliedImpulse(100), + m_jacSizeA(0), + m_jacSizeBoth(0) { - m_jac_size_A = (6 + bodyA->getNumLinks()); - m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); - m_pos_offset = ((1 + m_jac_size_both)*m_num_rows); - m_data.resize((2 + m_jac_size_both) * m_num_rows); + + if(bodyA) + { + if(bodyA->isMultiDof()) + m_jacSizeA = (6 + bodyA->getNumDofs()); + else + m_jacSizeA = (6 + bodyA->getNumLinks()); + } + + if(bodyB) + { + if(bodyB->isMultiDof()) + m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); + else + m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); + } + + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_data.resize((2 + m_jacSizeBoth) * m_numRows); } btMultiBodyConstraint::~btMultiBodyConstraint() @@ -42,15 +59,15 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS if (multiBodyA) { - const int ndofA = multiBodyA->getNumLinks() + 6; + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; //total dof count of tree A constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (constraintRow.m_deltaVelAindex <0) + if (constraintRow.m_deltaVelAindex <0) //if this multibody does not have a place allocated in m_deltaVelocities... { constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); //=> each constrained tree's dofs are represented in m_deltaVelocities } else { btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); @@ -58,18 +75,21 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS constraintRow.m_jacAindex = data.m_jacobians.size(); data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); for (int i=0;icalcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); } if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); if (constraintRow.m_deltaVelBindex <0) @@ -87,7 +107,10 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); } { @@ -101,7 +124,7 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; jacA = &data.m_jacobians[constraintRow.m_jacAindex]; lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; for (int i = 0; i < ndofA; ++i) @@ -113,7 +136,7 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS } if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; jacB = &data.m_jacobians[constraintRow.m_jacBindex]; lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; for (int i = 0; i < ndofB; ++i) @@ -161,14 +184,14 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS btVector3 vel1,vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; } if (multiBodyB) { - ndofB = multiBodyB->getNumLinks() + 6; + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; @@ -257,7 +280,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr if (multiBodyA) { - const int ndofA = multiBodyA->getNumLinks() + 6; + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); @@ -277,9 +300,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + 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); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); } else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); @@ -290,7 +319,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); if (solverConstraint.m_deltaVelBindex <0) @@ -306,8 +335,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); + if(multiBodyB->isMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); } else { btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); @@ -328,7 +363,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) @@ -347,7 +382,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr } if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) @@ -404,7 +439,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr btVector3 vel1,vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; @@ -417,7 +452,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr } if (multiBodyB) { - ndofB = multiBodyB->getNumLinks() + 6; + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; @@ -493,15 +528,20 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr erp = infoGlobal.m_erp; } - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; + //commented out on purpose, see below + //if (penetration>0) + //{ + // positionalError = 0; + // velocityError = -penetration / infoGlobal.m_timeStep; - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } + //} else + //{ + // positionalError = -penetration * erp/infoGlobal.m_timeStep; + //} + + //we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints) + //both are valid in general and definitely so in the case of a point2Point constraint + positionalError = -penetration * erp/infoGlobal.m_timeStep; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 97f5486d6..12b5a33f2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -28,8 +28,8 @@ struct btSolverInfo; struct btMultiBodyJacobianData { btAlignedObjectArray m_jacobians; - btAlignedObjectArray m_deltaVelocitiesUnitImpulse; - btAlignedObjectArray m_deltaVelocities; + btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension + btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI btAlignedObjectArray scratch_r; btAlignedObjectArray scratch_v; btAlignedObjectArray scratch_m; @@ -48,10 +48,10 @@ protected: int m_linkA; int m_linkB; - int m_num_rows; - int m_jac_size_A; - int m_jac_size_both; - int m_pos_offset; + int m_numRows; + int m_jacSizeA; + int m_jacSizeBoth; + int m_posOffset; bool m_isUnilateral; @@ -99,7 +99,7 @@ public: int getNumRows() const { - return m_num_rows; + return m_numRows; } btMultiBody* getMultiBodyA() @@ -116,12 +116,12 @@ public: // NOTE: ignored position for friction rows. btScalar getPosition(int row) const { - return m_data[m_pos_offset + row]; + return m_data[m_posOffset + row]; } void setPosition(int row, btScalar pos) { - m_data[m_pos_offset + row] = pos; + m_data[m_posOffset + row] = pos; } @@ -135,19 +135,19 @@ public: // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. btScalar* jacobianA(int row) { - return &m_data[m_num_rows + row * m_jac_size_both]; + return &m_data[m_numRows + row * m_jacSizeBoth]; } const btScalar* jacobianA(int row) const { - return &m_data[m_num_rows + (row * m_jac_size_both)]; + return &m_data[m_numRows + (row * m_jacSizeBoth)]; } btScalar* jacobianB(int row) { - return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } const btScalar* jacobianB(int row) const { - return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } btScalar getMaxAppliedImpulse() const diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 2a5dc815c..7826abd41 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -108,10 +108,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyA) { - ndofA = c.m_multiBodyA->getNumLinks() + 6; + ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; for (int i = 0; i < ndofA; ++i) deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } else + } else if(c.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); @@ -119,10 +119,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyB) { - ndofB = c.m_multiBodyB->getNumLinks() + 6; + ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; for (int i = 0; i < ndofB; ++i) deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } else + } else if(c.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); @@ -151,8 +151,11 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyA) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } else + if(c.m_multiBodyA->isMultiDof()) + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + else + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } else if(c.m_solverBodyIdA >= 0) { bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); @@ -160,8 +163,11 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyB) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } else + if(c.m_multiBodyB->isMultiDof()) + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + else + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } else if(c.m_solverBodyIdB >= 0) { bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } @@ -180,14 +186,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con if (c.m_multiBodyA) { - ndofA = c.m_multiBodyA->getNumLinks() + 6; + ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; for (int i = 0; i < ndofA; ++i) deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; } if (c.m_multiBodyB) { - ndofB = c.m_multiBodyB->getNumLinks() + 6; + ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; for (int i = 0; i < ndofB; ++i) deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; } @@ -257,7 +263,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { - const int ndofA = multiBodyA->getNumLinks() + 6; + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); @@ -277,9 +283,15 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + if(multiBodyA->isMultiDof()) + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + else + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); } else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); @@ -290,7 +302,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); if (solverConstraint.m_deltaVelBindex <0) @@ -306,8 +318,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + if(multiBodyB->isMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); } else { btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); @@ -328,7 +346,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) @@ -347,7 +365,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) @@ -404,7 +422,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btVector3 vel1,vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; @@ -417,7 +435,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } if (multiBodyB) { - ndofB = multiBodyB->getNumLinks() + 6; + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; @@ -452,7 +470,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); + if(multiBodyA->isMultiDof()) + multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); + else + multiBodyA->applyDeltaVee(deltaV,impulse); applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); } else { @@ -463,7 +484,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); + if(multiBodyB->isMultiDof()) + multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); + else + multiBodyB->applyDeltaVee(deltaV,impulse); applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); } else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 5cf197997..bdcd8c5f6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -451,7 +451,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if (!isSleeping) { - scratch_r.resize(bod->getNumLinks()+1); + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) scratch_v.resize(bod->getNumLinks()+1); scratch_m.resize(bod->getNumLinks()+1); @@ -463,7 +464,10 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); } - bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + if(bod->isMultiDof()) + bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + else + bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); } } } @@ -503,13 +507,14 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { int nLinks = bod->getNumLinks(); - ///base + num links + ///base + num m_links world_to_local.resize(nLinks+1); local_origin.resize(nLinks+1); - bod->stepPositions(timeStep); - - + if(bod->isMultiDof()) + bod->stepPositionsMultiDof(timeStep); + else + bod->stepPositions(timeStep); world_to_local[0] = bod->getWorldToBaseRot(); local_origin[0] = bod->getBasePos(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 5eb40f14b..21baf3d0e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) + //:btMultiBodyConstraint(body,0,link,-1,2,true), :btMultiBodyConstraint(body,body,link,link,2,true), m_lowerBound(lower), m_upperBound(upper) @@ -32,11 +33,14 @@ btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* bo // note: we rely on the fact that data.m_jacobians are // always initialized to zero by the Constraint ctor - // row 0: the lower bound - jacobianA(0)[6 + link] = 1; + unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); - // row 1: the upper bound - jacobianB(1)[6 + link] = -1; + // row 0: the lower bound + jacobianA(0)[offset] = 1; + // row 1: the upper bound + //jacobianA(1)[offset] = -1; + + jacobianB(1)[offset] = -1; } btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() { @@ -44,28 +48,34 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() int btMultiBodyJointLimitConstraint::getIslandIdA() const { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) + if(m_bodyA) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } } return -1; } int btMultiBodyJointLimitConstraint::getIslandIdB() const { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) + if(m_bodyB) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } } return -1; } @@ -79,7 +89,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint // directions were set in the ctor and never change. // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent // row 1: the upper bound setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); @@ -90,7 +100,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; - btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse); + btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,m_maxAppliedImpulse); { btScalar penetration = getPosition(row); btScalar positionalError = 0.f; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index c20663ff4..f612aa025 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) + //:btMultiBodyConstraint(body,0,link,-1,1,true), :btMultiBodyConstraint(body,body,link,link,1,true), m_desiredVelocity(desiredVelocity) { @@ -32,8 +33,11 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal // note: we rely on the fact that data.m_jacobians are // always initialized to zero by the Constraint ctor - // row 0: the lower bound - jacobianA(0)[6 + link] = 1; + unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); + + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; } btMultiBodyJointMotor::~btMultiBodyJointMotor() { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index eaaf87d72..d89942747 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -24,6 +24,303 @@ enum btMultiBodyLinkFlags { BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 }; + +//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS +#define TEST_SPATIAL_ALGEBRA_LAYER + +// +// Various spatial helper functions +// + +namespace { + +#ifdef TEST_SPATIAL_ALGEBRA_LAYER + + struct btSpatialForceVector + { + btVector3 m_topVec, m_bottomVec; + // + btSpatialForceVector() { setZero(); } + btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} + btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + setValue(ax, ay, az, lx, ly, lz); + } + // + void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } + void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); + } + // + void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; + m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; + } + // + const btVector3 & getLinear() const { return m_topVec; } + const btVector3 & getAngular() const { return m_bottomVec; } + // + void setLinear(const btVector3 &linear) { m_topVec = linear; } + void setAngular(const btVector3 &angular) { m_bottomVec = angular; } + // + void addAngular(const btVector3 &angular) { m_bottomVec += angular; } + void addLinear(const btVector3 &linear) { m_topVec += linear; } + // + void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + // + btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } + btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } + btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } + btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } + btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } + //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } + }; + + struct btSpatialMotionVector + { + btVector3 m_topVec, m_bottomVec; + // + btSpatialMotionVector() { setZero(); } + btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} + // + void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } + void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); + } + // + void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } + void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) + { + m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; + m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; + } + // + const btVector3 & getAngular() const { return m_topVec; } + const btVector3 & getLinear() const { return m_bottomVec; } + // + void setAngular(const btVector3 &angular) { m_topVec = angular; } + void setLinear(const btVector3 &linear) { m_bottomVec = linear; } + // + void addAngular(const btVector3 &angular) { m_topVec += angular; } + void addLinear(const btVector3 &linear) { m_bottomVec += linear; } + // + void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } + // + btScalar dot(const btSpatialForceVector &b) const + { + return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); + } + // + template + void cross(const SpatialVectorType &b, SpatialVectorType &out) const + { + out.m_topVec = m_topVec.cross(b.m_topVec); + out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); + } + template + SpatialVectorType cross(const SpatialVectorType &b) const + { + SpatialVectorType out; + out.m_topVec = m_topVec.cross(b.m_topVec); + out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); + return out; + } + // + btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } + btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } + btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } + btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } + btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } + }; + + struct btSymmetricSpatialDyad + { + btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; + // + btSymmetricSpatialDyad() { setIdentity(); } + btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } + // + void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) + { + m_topLeftMat = topLeftMat; + m_topRightMat = topRightMat; + m_bottomLeftMat = bottomLeftMat; + } + // + void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) + { + m_topLeftMat += topLeftMat; + m_topRightMat += topRightMat; + m_bottomLeftMat += bottomLeftMat; + } + // + void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } + // + btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) + { + m_topLeftMat -= mat.m_topLeftMat; + m_topRightMat -= mat.m_topRightMat; + m_bottomLeftMat -= mat.m_bottomLeftMat; + return *this; + } + // + btSpatialForceVector operator * (const btSpatialMotionVector &vec) + { + return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); + } + }; + + struct btSpatialTransformationMatrix + { + btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; + btVector3 m_trnVec; + // + enum eOutputOperation + { + None = 0, + Add = 1, + Subtract = 2 + }; + // + template + void transform( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat * inVec.m_topVec; + outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat * inVec.m_topVec; + outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat * inVec.m_topVec; + outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; + } + + } + + template + void transformRotationOnly( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; + } + + } + + template + void transformInverse( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); + } + } + + void transformInverse( const btSymmetricSpatialDyad &inMat, + btSymmetricSpatialDyad &outMat, + eOutputOperation outOp = None) + { + const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], + m_trnVec[2], 0, -m_trnVec[0], + -m_trnVec[1], m_trnVec[0], 0); + + + if(outOp == None) + { + outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + else if(outOp == Add) + { + outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + else if(outOp == Subtract) + { + outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; + outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; + outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; + } + } + + template + SpatialVectorType operator * (const SpatialVectorType &vec) + { + SpatialVectorType out; + transform(vec, out); + return out; + } + }; + + template + void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) + { + //output op maybe? + + out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); + out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); + out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); + //maybe simple a*spatTranspose(a) would be nicer? + } + + template + btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) + { + btSymmetricSpatialDyad out; + + out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); + out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); + out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); + + return out; + //maybe simple a*spatTranspose(a) would be nicer? + } + +#endif +} + // // Link struct // @@ -33,75 +330,155 @@ struct btMultibodyLink BT_DECLARE_ALIGNED_ALLOCATOR(); - btScalar joint_pos; // qi + btScalar m_mass; // mass of link + btVector3 m_inertia; // inertia of link (local frame; diagonal) - btScalar mass; // mass of link - btVector3 inertia; // inertia of link (local frame; diagonal) + int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. - int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. + btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. - btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. + btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only. - // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. - // for prismatic: axis_top = zero; - // axis_bottom = unit vector along the joint axis. - // for revolute: axis_top = unit vector along the rotation axis (u); - // axis_bottom = u cross d_vector. - btVector3 axis_top; - btVector3 axis_bottom; - - btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only. - - // e_vector is constant, but depends on the joint type + // m_eVector is constant, but depends on the joint type // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) // revolute: vector from parent's COM to the pivot point, in PARENT's frame. - btVector3 e_vector; + btVector3 m_eVector; - bool is_revolute; // true = revolute, false = prismatic + enum eFeatherstoneJointType + { + eRevolute = 0, + ePrismatic = 1, + eSpherical = 2, +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS + ePlanar = 3, +#endif + eInvalid + }; - btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame - btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame. + eFeatherstoneJointType m_jointType; + int m_dofCount, m_posVarCount; //redundant but handy - btVector3 applied_force; // In WORLD frame - btVector3 applied_torque; // In WORLD frame - btScalar joint_torque; + // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. + // for prismatic: m_axesTop[0] = zero; + // m_axesBottom[0] = unit vector along the joint axis. + // for revolute: m_axesTop[0] = unit vector along the rotation axis (u); + // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint) + // + // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes) + // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint) + // + // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion + // m_axesTop[1][2] = zero + // m_axesBottom[0] = zero + // m_axesBottom[1][2] = unit vectors along the translational axes on that plane +#ifndef TEST_SPATIAL_ALGEBRA_LAYER + btVector3 m_axesTop[6]; + btVector3 m_axesBottom[6]; + void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; } + void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; } + void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); } + void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); } + const btVector3 & getAxisTop(int dof) const { return m_axesTop[dof]; } + const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; } +#else + 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); } + const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; } + const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } +#endif + + int m_dofOffset; + + btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + btVector3 m_appliedForce; // In WORLD frame + btVector3 m_appliedTorque; // In WORLD frame + + btScalar m_jointPos[7]; + btScalar m_jointTorque[6]; //TODO class btMultiBodyLinkCollider* m_collider; int m_flags; // ctor: set some sensible defaults btMultibodyLink() - : joint_pos(0), - mass(1), - parent(-1), - zero_rot_parent_to_this(1, 0, 0, 0), - is_revolute(false), - cached_rot_parent_to_this(1, 0, 0, 0), - joint_torque(0), + : m_mass(1), + m_parent(-1), + m_zeroRotParentToThis(1, 0, 0, 0), + m_cachedRotParentToThis(1, 0, 0, 0), m_collider(0), - m_flags(0) + m_flags(0), + m_dofCount(0), + m_posVarCount(0), + m_jointType(btMultibodyLink::eInvalid) { - inertia.setValue(1, 1, 1); - axis_top.setValue(0, 0, 0); - axis_bottom.setValue(1, 0, 0); - d_vector.setValue(0, 0, 0); - e_vector.setValue(0, 0, 0); - cached_r_vector.setValue(0, 0, 0); - applied_force.setValue( 0, 0, 0); - applied_torque.setValue(0, 0, 0); + m_inertia.setValue(1, 1, 1); + setAxisTop(0, 0., 0., 0.); + setAxisBottom(0, 1., 0., 0.); + m_dVector.setValue(0, 0, 0); + m_eVector.setValue(0, 0, 0); + m_cachedRVector.setValue(0, 0, 0); + m_appliedForce.setValue( 0, 0, 0); + m_appliedTorque.setValue(0, 0, 0); + // + m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f; + m_jointPos[3] = 1.f; //"quat.w" + m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f; } - // routine to update cached_rot_parent_to_this and cached_r_vector + // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCache() { - if (is_revolute) + //multidof + if (m_jointType == eRevolute) { - cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this; - cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector); + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); } else { - // cached_rot_parent_to_this never changes, so no need to update - cached_r_vector = e_vector + joint_pos * axis_bottom; + // m_cachedRotParentToThis never changes, so no need to update + m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0); + } + } + + void updateCacheMultiDof() + { + switch(m_jointType) + { + case eRevolute: + { + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + m_cachedRotParentToThis = btQuaternion(m_jointPos[0], m_jointPos[1], m_jointPos[2], -m_jointPos[3]) * m_zeroRotParentToThis; + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } +#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS + case ePlanar: + { + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-m_jointPos[0]), m_jointPos[1] * m_axesBottom[1] + m_jointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } +#endif } } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 3cc27a07a..f5622e062 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -74,14 +74,14 @@ public: if (m_link>=0) { const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link) + if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) return false; } if (other->m_link>=0) { const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link) + if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) return false; } return true; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index a16455bfb..029f33449 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "BulletDynamics/Dynamics/btRigidBody.h" btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,3,false), + :btMultiBodyConstraint(body,0,link,-1,3,false), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 15d089956..f48935d74 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -28,6 +28,8 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); + btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1) + {} int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 btVector3 m_relpos1CrossNormal; From cb556f95254e961094564ddabd10e570be1d06ba Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:51:42 +0100 Subject: [PATCH 02/17] dirty changes - stabilization hacks --- .../BulletMultiBodyDemos.cpp | 2 +- .../Featherstone/btMultiBody.cpp | 170 +++++++++++++++--- .../Featherstone/btMultiBodyConstraint.cpp | 4 +- .../Featherstone/btMultiBodyJointMotor.cpp | 6 +- .../Featherstone/btMultiBodyJointMotor.h | 2 +- .../Featherstone/btMultiBodyLink.h | 28 ++- 6 files changed, 182 insertions(+), 30 deletions(-) diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 1036809f1..368953eb2 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -396,7 +396,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn { if (1) { - btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); + btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,0,500000); world->addMultiBodyConstraint(con); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a25a9c988..3fb41f89b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1111,6 +1111,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + static btSpatialTransformationMatrix fromWorld; + fromWorld.m_trnVec.setZero(); ///////////////// // ptr to the joint accel part of the output @@ -1136,7 +1138,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce)); //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 10.; + btScalar linDampMult = 1., angDampMult = 1.; zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); @@ -1163,33 +1165,76 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, rot_from_world[0] = rot_from_parent[0]; - for (int i = 0; i < num_links; ++i) { + // + bool useGlobalVelocities = false; + for (int i = 0; i < num_links; ++i) { const int parent = m_links[i].m_parent; rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); - - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - fromParent.transform(spatVel[parent+1], spatVel[i+1]); - //nice alternative below (using operator *) but it generates temps + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + + ////clamp parent's omega + //btScalar parOmegaMod = spatVel[parent+1].getAngular().length(); + //btScalar parOmegaModMax = 0.1; + //if(parOmegaMod > parOmegaModMax) + //{ + // //btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear()); + // btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod); + // fromParent.transform(clampedParVel, spatVel[i+1]); + // spatVel[parent+1] *= (parOmegaModMax / parOmegaMod); + //} + //else + { + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + } ////////////////////////////////////////////////////////////// - // now set vhat_i to its true value by doing + //if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical) + //{ + // btScalar mod2 = 0; + // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + // mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof]; + + // btScalar angvel = sqrt(mod2); + // btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075; + // btScalar step = 1; //dt + // if (angvel*step > maxAngVel) + // { + // btScalar * qd = getJointVelMultiDof(i); + // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + // qd[dof] *= (maxAngVel/step) /angvel; + // } + //} + + // now set vhat_i to its true value by doing // vhat_i += qidot * shat_i - spatJointVel.setZero(); - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - - // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - spatVel[i+1] += spatJointVel; - // - // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint - //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; + if(!useGlobalVelocities) + { + spatJointVel.setZero(); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; + + // + // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint + //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; + + } + else + { + fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]); + fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel); + } // we can now calculate chat_i - spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); + spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); // calculate zhat_i^A // @@ -1197,7 +1242,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce)); // //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 10.; + btScalar linDampMult = 1., angDampMult = 1.; zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); @@ -1219,6 +1264,15 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular())); // zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + ////clamp parent's omega + //btScalar parOmegaMod = temp.length(); + //btScalar parOmegaModMax = 1000; + //if(parOmegaMod > parOmegaModMax) + // temp *= parOmegaModMax / parOmegaMod; + //zeroAccSpatFrc[i+1].addLinear(temp); + //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); @@ -1360,6 +1414,12 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // now do the loop over the m_links for (int i = 0; i < num_links; ++i) { + // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar) + // a = apar + cor + Sqdd + //or + // qdd = D^{-1} * (Y - h^{T}*(apar+cor)) + // a = apar + Sqdd + const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1369,13 +1429,14 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, { btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); } btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + //D^{-1} * (Y - h^{T}*apar) mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - spatAcc[i+1] += spatCoriolisAcc[i]; + spatAcc[i+1] += spatCoriolisAcc[i]; for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; @@ -1411,7 +1472,68 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, ///////////////// // Final step: add the accelerations (times dt) to the velocities. - applyDeltaVeeMultiDof(output, dt); + applyDeltaVeeMultiDof(output, dt); + + ///// + //btScalar angularThres = 1; + //btScalar maxAngVel = 0.; + //bool scaleDown = 1.; + //for(int link = 0; link < m_links.size(); ++link) + //{ + // if(spatVel[link+1].getAngular().length() > maxAngVel) + // { + // maxAngVel = spatVel[link+1].getAngular().length(); + // scaleDown = angularThres / spatVel[link+1].getAngular().length(); + // break; + // } + //} + + //if(scaleDown != 1.) + //{ + // for(int link = 0; link < m_links.size(); ++link) + // { + // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical) + // { + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // getJointVelMultiDof(link)[dof] *= scaleDown; + // } + // } + //} + ///// + + /////////////////// + if(useGlobalVelocities) + { + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done + //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + ///////////////////////////////////////////////////////////// + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + spatJointVel.setZero(); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; + + + fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); + } + } + } #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 1439755be..609f92d63 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -197,7 +197,9 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; } - + for (int i = 6; i < ndofA ; ++i) + printf("%.4f ", multiBodyA->getVelocityVector()[i]); + printf("\nrel_vel = %.4f\n------------\n", rel_vel); constraintRow.m_friction = 0.f; constraintRow.m_appliedImpulse = 0.f; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index f612aa025..527f2c011 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -21,11 +21,13 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), :btMultiBodyConstraint(body,body,link,link,1,true), m_desiredVelocity(desiredVelocity) { + btAssert(linkDoF < body->getLink(link).m_dofCount); + m_maxAppliedImpulse = maxMotorImpulse; // the data.m_jacobians never change, so may as well // initialize them here @@ -33,7 +35,7 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal // note: we rely on the fact that data.m_jacobians are // always initialized to zero by the Constraint ctor - unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); + unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset + linkDoF : link); // row 0: the lower bound // row 0: the lower bound diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 61ee4663b..551813807 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -30,7 +30,7 @@ protected: public: - btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse); + btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse); virtual ~btMultiBodyJointMotor(); virtual int getIslandIdA() const; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index d89942747..7566bda53 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -133,6 +133,7 @@ namespace { // btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } + btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } @@ -256,6 +257,29 @@ namespace { } } + template + void transformInverseRotationOnly( const SpatialVectorType &inVec, + SpatialVectorType &outVec, + eOutputOperation outOp = None) + { + if(outOp == None) + { + outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; + } + else if(outOp == Add) + { + outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; + } + else if(outOp == Subtract) + { + outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; + outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; + } + + } + void transformInverse( const btSymmetricSpatialDyad &inMat, btSymmetricSpatialDyad &outMat, eOutputOperation outOp = None) @@ -342,7 +366,9 @@ struct btMultibodyLink // m_eVector is constant, but depends on the joint type // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) // revolute: vector from parent's COM to the pivot point, in PARENT's frame. - btVector3 m_eVector; + btVector3 m_eVector; + + btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; enum eFeatherstoneJointType { From e5372f37127c06dde650186718c757d97dd60233 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:56:46 +0100 Subject: [PATCH 03/17] first experiments with RK4 --- .../Featherstone/btMultiBody.cpp | 103 +++++++------ src/BulletDynamics/Featherstone/btMultiBody.h | 19 ++- .../btMultiBodyConstraintSolver.cpp | 8 + .../Featherstone/btMultiBodyDynamicsWorld.cpp | 138 +++++++++++++++++- .../Featherstone/btMultiBodyLink.h | 16 +- 5 files changed, 223 insertions(+), 61 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 3fb41f89b..8e69ba679 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -100,7 +100,9 @@ btMultiBody::btMultiBody(int n_links, m_useGyroTerm(true), m_maxAppliedImpulse(1000.f), m_hasSelfCollision(true), - m_dofCount(n_links) + m_dofCount(n_links), + __posUpdated(false), + m_posVarCnt(-1) //-1 => not calculated yet/invalid { m_links.resize(n_links); @@ -132,7 +134,9 @@ btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass, m_useGyroTerm(true), m_maxAppliedImpulse(1000.f), m_hasSelfCollision(true), - m_dofCount(n_dofs) + m_dofCount(n_dofs), + __posUpdated(false), + m_posVarCnt(-1) //-1 => not calculated yet/invalid { m_links.resize(n_links); @@ -1264,7 +1268,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular())); // zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); ////clamp parent's omega //btScalar parOmegaMod = temp.length(); //btScalar parOmegaModMax = 1000; @@ -1272,6 +1276,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // temp *= parOmegaModMax / parOmegaMod; //zeroAccSpatFrc[i+1].addLinear(temp); //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //temp = spatCoriolisAcc[i].getLinear(); + //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); @@ -1472,7 +1478,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, ///////////////// // Final step: add the accelerations (times dt) to the velocities. - applyDeltaVeeMultiDof(output, dt); + if(dt > 0.) + applyDeltaVeeMultiDof(output, dt); ///// //btScalar angularThres = 1; @@ -1501,38 +1508,38 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, //} ///// - /////////////////// - if(useGlobalVelocities) - { - for (int i = 0; i < num_links; ++i) - { - const int parent = m_links[i].m_parent; - //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done - //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done - - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - fromWorld.m_rotMat = rot_from_world[i+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - fromParent.transform(spatVel[parent+1], spatVel[i+1]); - //nice alternative below (using operator *) but it generates temps - ///////////////////////////////////////////////////////////// + ///////////////////// + //if(useGlobalVelocities) + //{ + // for (int i = 0; i < num_links; ++i) + // { + // const int parent = m_links[i].m_parent; + // //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done + // //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done + // + // fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + // fromWorld.m_rotMat = rot_from_world[i+1]; + // + // // vhat_i = i_xhat_p(i) * vhat_p(i) + // fromParent.transform(spatVel[parent+1], spatVel[i+1]); + // //nice alternative below (using operator *) but it generates temps + // ///////////////////////////////////////////////////////////// - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - spatJointVel.setZero(); + // // now set vhat_i to its true value by doing + // // vhat_i += qidot * shat_i + // spatJointVel.setZero(); - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - - // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - spatVel[i+1] += spatJointVel; + // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + // spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + // + // // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + // spatVel[i+1] += spatJointVel; - fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); - fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); - } - } + // fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + // fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); + // } + //} } #endif @@ -2464,8 +2471,8 @@ void btMultiBody::stepPositions(btScalar dt) } } -void btMultiBody::stepPositionsMultiDof(btScalar dt) -{ +void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) +{ int num_links = getNumLinks(); // step position by adding dt * velocity btVector3 v = getBaseVel(); @@ -2523,40 +2530,48 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt) // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { + btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); + switch(m_links[i].m_jointType) { case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { - btScalar jointVel = getJointVelMultiDof(i)[0]; - m_links[i].m_jointPos[0] += dt * jointVel; + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { - btScalar *pJointVel = getJointVelMultiDof(i); + //btScalar *pJointVel = getJointVelMultiDof(i); static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - static btQuaternion jointOri; jointOri.setValue(m_links[i].m_jointPos[0], m_links[i].m_jointPos[1], m_links[i].m_jointPos[2], m_links[i].m_jointPos[3]); + static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); pQuatUpdateFun(jointVel, jointOri, false, dt); - m_links[i].m_jointPos[0] = jointOri.x(); m_links[i].m_jointPos[1] = jointOri.y(); m_links[i].m_jointPos[2] = jointOri.z(); m_links[i].m_jointPos[3] = jointOri.w(); + pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); break; } #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS case btMultibodyLink::ePlanar: { - m_links[i].m_jointPos[0] += dt * getJointVelMultiDof(i)[0]; + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), m_links[i].m_jointPos[0]), q0_coors_qd1qd2); - m_links[i].m_jointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - m_links[i].m_jointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; break; } #endif } - m_links[i].updateCacheMultiDof(); + m_links[i].updateCacheMultiDof(pq); + + if(pq) + pq += m_links[i].m_posVarCount; + if(pqd) + pqd += m_links[i].m_dofCount; } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 048e3341c..8b3b33bc9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -138,6 +138,7 @@ public: int getNumLinks() const { return m_links.size(); } int getNumDofs() const { return m_dofCount; } + int getNumPosVars() const { return m_posVarCnt; } btScalar getBaseMass() const { return m_baseMass; } const btVector3 & getBaseInertia() const { return m_baseInertia; } btScalar getLinkMass(int i) const; @@ -375,7 +376,7 @@ public: // timestep the positions (given current velocities). void stepPositions(btScalar dt); - void stepPositionsMultiDof(btScalar dt); + void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); // @@ -481,6 +482,8 @@ public: bool isMultiDof() { return m_isMultiDof; } void forceMultiDof(); + bool __posUpdated; + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -492,7 +495,17 @@ private: void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; #endif - void updateLinksDofOffsets() { int dofOffset = 0; for(int bidx = 0; bidx < m_links.size(); ++bidx) { m_links[bidx].m_dofOffset = dofOffset; dofOffset += m_links[bidx].m_dofCount; } } + void updateLinksDofOffsets() + { + int dofOffset = 0, cfgOffset = 0; + for(int bidx = 0; bidx < m_links.size(); ++bidx) + { + m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; + dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; + } + + m_posVarCnt = cfgOffset; + } void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; @@ -511,7 +524,7 @@ private: btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; - int m_dofCount; + int m_dofCount, m_posVarCnt; // // realBuf: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 7826abd41..1e4004af3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -36,6 +36,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //if (iteration < constraint.m_overrideNumSolverIterations) //resolveSingleConstraintRowGenericMultiBody(constraint); resolveSingleConstraintRowGeneric(constraint); + if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false; + if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false; } //solve featherstone normal contact @@ -44,6 +46,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; if (iteration < infoGlobal.m_numIterations) resolveSingleConstraintRowGeneric(constraint); + + if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false; + if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false; } //solve featherstone frictional contact @@ -60,6 +65,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; resolveSingleConstraintRowGeneric(frictionConstraint); + + if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->__posUpdated = false; + if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->__posUpdated = false; } } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index bdcd8c5f6..1e6d7a7fe 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); m_bodies.resize(0); @@ -392,9 +394,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () delete m_solverMultiBodyIslandCallback; } - - - void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { @@ -464,8 +463,130 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); } + bool rk = false; + bool doNotUpdatePos = false; + if(bod->isMultiDof()) - bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + { + if(!rk) + bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + else + { + // + btAlignedObjectArray scratch_r2; scratch_r2.resize(2*bod->getNumPosVars() + 8*bod->getNumDofs()); + //convenience + btScalar *pMem = &scratch_r2[0]; + btScalar *scratch_q0 = pMem; pMem += bod->getNumPosVars(); + btScalar *scratch_qx = pMem; pMem += bod->getNumPosVars(); + btScalar *scratch_qd0 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qd1 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qd2 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qd3 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qdd0 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qdd1 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qdd2 = pMem; pMem += bod->getNumDofs(); + btScalar *scratch_qdd3 = pMem; pMem += bod->getNumDofs(); + btAssert((pMem - (2*bod->getNumPosVars() + 8*bod->getNumDofs())) == &scratch_r2[0]); + + //copy q0 to scratch_q0 and qd0 to scratch_qd0 + for(int link = 0; link < bod->getNumLinks(); ++link) + { + for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + + for(int dof = 0; dof < bod->getLink(link).m_dofCount; ++dof) + scratch_qd0[bod->getLink(link).m_dofOffset + dof] = bod->getVelocityVector()[6 + bod->getLink(link).m_dofOffset + dof]; + } + + auto pResetQx = [&scratch_qx, &scratch_q0, &bod]() + { + for(int dof = 0; dof < bod->getNumPosVars(); ++dof) + scratch_qx[dof] = scratch_q0[dof]; + }; + + auto pEulerIntegrate = [](btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) + { + for(int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + }; + // + auto pCopyToVelocitVector = [](btMultiBody *pBody, const btScalar *pData) + { + btScalar *pVel = const_cast(pBody->getVelocityVector()); + + for(int i = 0; i < pBody->getNumDofs(); ++i) + pVel[6+i] = pData[i]; + }; + // + auto pCopy = [](const btScalar *pSrc, btScalar *pDst, int start, int size) + { + for(int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + }; + // + + btScalar h = solverInfo.m_timeStep; + //calc qdd0 from: q0 & qd0 + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(&scratch_r[0], scratch_qdd0, bod->getNumDofs() + 6, bod->getNumDofs()); + //calc q1 = q0 + h/2 * qd0 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); + //calc qd1 = qd0 + h/2 * qdd0 + pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, bod->getNumDofs()); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocitVector(bod, scratch_qd1); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(&scratch_r[0], scratch_qdd1, bod->getNumDofs() + 6, bod->getNumDofs()); + //calc q2 = q0 + h/2 * qd1 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); + //calc qd2 = qd0 + h/2 * qdd1 + pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, bod->getNumDofs()); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocitVector(bod, scratch_qd2); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(&scratch_r[0], scratch_qdd2, bod->getNumDofs() + 6, bod->getNumDofs()); + //calc q3 = q0 + h * qd2 + pResetQx(); + bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); + //calc qd3 = qd0 + h * qdd2 + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, bod->getNumDofs()); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocitVector(bod, scratch_qd3); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(&scratch_r[0], scratch_qdd3, bod->getNumDofs() + 6, bod->getNumDofs()); + + // + //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + //btAlignedObjectArray res_qd; res_qd.resize(bod->getNumDofs()); + btAlignedObjectArray delta_q; delta_q.resize(bod->getNumDofs()); + btAlignedObjectArray delta_qd; delta_qd.resize(6+bod->getNumDofs()); + for(int i = 0; i < bod->getNumDofs(); ++i) + { + delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); + delta_qd[6+i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); + //delta_q[i] = h*scratch_qd0[i]; + //delta_qd[6+i] = h*scratch_qdd0[i]; + } + + // + if(!doNotUpdatePos) + { + bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->__posUpdated = true; + } + // + pCopyToVelocitVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + // + //delete [] scratch_q; + } + } else bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); } @@ -512,9 +633,12 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) local_origin.resize(nLinks+1); if(bod->isMultiDof()) - bod->stepPositionsMultiDof(timeStep); + { + if(!bod->__posUpdated) + bod->stepPositionsMultiDof(timeStep); + } else - bod->stepPositions(timeStep); + bod->stepPositions(timeStep); world_to_local[0] = bod->getWorldToBaseRot(); local_origin[0] = bod->getBasePos(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 7566bda53..af926f57f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -416,7 +416,7 @@ struct btMultibodyLink const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } #endif - int m_dofOffset; + int m_dofOffset, m_cfgOffset; btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. @@ -471,13 +471,15 @@ struct btMultibodyLink } } - void updateCacheMultiDof() + void updateCacheMultiDof(btScalar *pq = 0) { + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + switch(m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); break; @@ -485,13 +487,13 @@ struct btMultibodyLink case ePrismatic: { // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0); + m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(m_jointPos[0], m_jointPos[1], m_jointPos[2], -m_jointPos[3]) * m_zeroRotParentToThis; + m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); break; @@ -499,8 +501,8 @@ struct btMultibodyLink #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-m_jointPos[0]), m_jointPos[1] * m_axesBottom[1] + m_jointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector); + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector); break; } From 4eac9a11f355bad2fef6cda99cc67322ec50e23b Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:58:31 +0100 Subject: [PATCH 04/17] made the multiDof-singleDof disctinction a bit cleaner --- .../Featherstone/btMultiBody.cpp | 83 ++++++++----------- src/BulletDynamics/Featherstone/btMultiBody.h | 41 ++++----- 2 files changed, 51 insertions(+), 73 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 8e69ba679..d4cafe36d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -85,7 +85,8 @@ btMultiBody::btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, - bool canSleep) + bool canSleep, + bool multiDof) : m_baseQuat(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -101,54 +102,24 @@ btMultiBody::btMultiBody(int n_links, m_maxAppliedImpulse(1000.f), m_hasSelfCollision(true), m_dofCount(n_links), - __posUpdated(false), - m_posVarCnt(-1) //-1 => not calculated yet/invalid + __posUpdated(false), + m_isMultiDof(multiDof), + m_posVarCnt(0) { + + if(!m_isMultiDof) + { + m_vectorBuf.resize(2*n_links); + m_realBuf.resize(6 + 2*n_links); + m_posVarCnt = n_links; + } + m_links.resize(n_links); - - m_vectorBuf.resize(2*n_links); - m_matrixBuf.resize(n_links + 1); - m_realBuf.resize(6 + 2*n_links); - m_basePos.setValue(0, 0, 0); - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); - - m_isMultiDof = false; -} - -btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass, - const btVector3 &inertia, - bool fixedBase, - bool canSleep) - : m_baseQuat(0, 0, 0, 1), - m_baseMass(mass), - m_baseInertia(inertia), - - m_fixedBase(fixedBase), - m_awake(true), - m_canSleep(canSleep), - m_sleepTimer(0), - m_baseCollider(0), - m_linearDamping(0.04f), - m_angularDamping(0.04f), - m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_hasSelfCollision(true), - m_dofCount(n_dofs), - __posUpdated(false), - m_posVarCnt(-1) //-1 => not calculated yet/invalid -{ - m_links.resize(n_links); - - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(n_links + 1); - m_basePos.setValue(0, 0, 0); + m_basePos.setValue(0, 0, 0); m_baseForce.setValue(0, 0, 0); m_baseTorque.setValue(0, 0, 0); - - m_isMultiDof = n_links != n_dofs; } btMultiBody::~btMultiBody() @@ -164,6 +135,12 @@ void btMultiBody::setupPrismatic(int i, const btVector3 &parentComToThisComOffset, bool disableParentCollision) { + if(m_isMultiDof) + { + m_dofCount += 1; + m_posVarCnt += 1; + } + m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; m_links[i].m_parent = parent; @@ -201,6 +178,12 @@ void btMultiBody::setupRevolute(int i, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { + if(m_isMultiDof) + { + m_dofCount += 1; + m_posVarCnt += 1; + } + m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; m_links[i].m_parent = parent; @@ -240,6 +223,8 @@ void btMultiBody::setupSpherical(int i, bool disableParentCollision) { btAssert(m_isMultiDof); + m_dofCount += 3; + m_posVarCnt += 4; m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; @@ -280,6 +265,8 @@ void btMultiBody::setupPlanar(int i, bool disableParentCollision) { btAssert(m_isMultiDof); + m_dofCount += 3; + m_posVarCnt += 3; m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; @@ -315,14 +302,12 @@ void btMultiBody::setupPlanar(int i, } #endif -void btMultiBody::forceMultiDof() +void btMultiBody::finalizeMultiDof() { - if(m_isMultiDof) return; + btAssert(m_isMultiDof); - m_isMultiDof = true; - - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) updateLinksDofOffsets(); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 8b3b33bc9..41cb783d3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -49,14 +49,9 @@ public: btScalar mass, // mass of base const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep); - - btMultiBody(int n_links, // NOT including the base - int n_dofs, // NOT including 6 floating base dofs - btScalar mass, // mass of base - const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal - bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep); + bool canSleep, + bool multiDof = false + ); ~btMultiBody(); @@ -356,20 +351,20 @@ public: // printf("%.4f ", delta_vee[dof]*multiplier); //printf("\n"); - btScalar sum = 0; - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; - } - btScalar l = btSqrt(sum); + //btScalar sum = 0; + //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + //{ + // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; + //} + //btScalar l = btSqrt(sum); - if (l>m_maxAppliedImpulse) - { - multiplier *= m_maxAppliedImpulse/l; - } + //if (l>m_maxAppliedImpulse) + //{ + // multiplier *= m_maxAppliedImpulse/l; + //} - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { m_realBuf[dof] += delta_vee[dof] * multiplier; } } @@ -480,7 +475,7 @@ public: } bool isMultiDof() { return m_isMultiDof; } - void forceMultiDof(); + void finalizeMultiDof(); bool __posUpdated; @@ -503,8 +498,6 @@ private: m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; } - - m_posVarCnt = cfgOffset; } void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; @@ -529,7 +522,7 @@ private: // // realBuf: // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) + // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] // 6+num_links num_links D // // vectorBuf: From c5594a58263f55cd0e7d76c477eb13a0c1f471e4 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:59:48 +0100 Subject: [PATCH 05/17] a bit of rk4 clean-up --- .../Featherstone/btMultiBody.cpp | 4 ++-- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 21 +++++++++++++++---- .../Featherstone/btMultiBodyDynamicsWorld.h | 4 ++++ 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index d4cafe36d..4b06aec7b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -306,8 +306,8 @@ void btMultiBody::finalizeMultiDof() { btAssert(m_isMultiDof); - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + m_posVarCnt); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) updateLinksDofOffsets(); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 1e6d7a7fe..cf5c5edc5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -381,7 +381,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) + m_multiBodyConstraintSolver(constraintSolver), m_useRK4ForMultiBodies(false) { //split impulse is not yet supported for Featherstone hierarchies getSolverInfo().m_splitImpulse = false; @@ -463,12 +463,11 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); } - bool rk = false; bool doNotUpdatePos = false; if(bod->isMultiDof()) { - if(!rk) + if(!m_useRK4ForMultiBodies) bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); else { @@ -577,7 +576,13 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // if(!doNotUpdatePos) { - bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + btScalar *pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + + for(int i = 0; i < bod->getNumPosVars(); ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); bod->__posUpdated = true; } // @@ -636,6 +641,14 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { if(!bod->__posUpdated) bod->stepPositionsMultiDof(timeStep); + else + { + btScalar *pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->__posUpdated = false; + } } else bod->stepPositions(timeStep); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index a4d1a1161..590906e16 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -34,6 +34,7 @@ protected: btAlignedObjectArray m_sortedMultiBodyConstraints; btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + bool m_useRK4ForMultiBodies; virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); @@ -52,5 +53,8 @@ public: virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); + + void useRK4IntegrationForMultiBodies(bool use) { m_useRK4ForMultiBodies = use; } + bool isUsingRK4IntegrationForMultiBodies() const { return m_useRK4ForMultiBodies; } }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H From 66fdc1704bf5f9e160a54bbdef6625309ff38f9b Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:01:03 +0100 Subject: [PATCH 06/17] RK4 for floating systems too --- .../Featherstone/btMultiBody.cpp | 53 ++++++-- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 115 ++++++++++-------- 2 files changed, 106 insertions(+), 62 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 4b06aec7b..e0dac8c3a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -306,8 +306,8 @@ void btMultiBody::finalizeMultiDof() { btAssert(m_isMultiDof); - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + m_posVarCnt); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) updateLinksDofOffsets(); } @@ -1058,7 +1058,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, scratch_v.resize(8*num_links + 6); scratch_m.resize(4*num_links + 4); - btScalar * r_ptr = &scratch_r[0]; + //btScalar * r_ptr = &scratch_r[0]; btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results btVector3 * v_ptr = &scratch_v[0]; @@ -1188,14 +1188,14 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) // mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof]; - // btScalar angvel = sqrt(mod2); - // btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075; - // btScalar step = 1; //dt - // if (angvel*step > maxAngVel) - // { + // btScalar angvel = sqrt(mod2); + // btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075; + // btScalar step = 1; //dt + // if (angvel*step > maxAngVel) + // { // btScalar * qd = getJointVelMultiDof(i); // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - // qd[dof] *= (maxAngVel/step) /angvel; + // qd[dof] *= (maxAngVel/step) /angvel; // } //} @@ -2460,8 +2460,15 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd { int num_links = getNumLinks(); // step position by adding dt * velocity - btVector3 v = getBaseVel(); - m_basePos += dt * v; + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + // + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -2510,7 +2517,28 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } pQuatUpdateFun; /////////////////////////////// - pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + // + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + + if(pq) + pq += 7; + if(pqd) + pqd += 6; // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) @@ -2529,7 +2557,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::eSpherical: { - //btScalar *pJointVel = getJointVelMultiDof(i); static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); pQuatUpdateFun(jointVel, jointOri, false, dt); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cf5c5edc5..9c2e0387d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -472,34 +472,46 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) else { // - btAlignedObjectArray scratch_r2; scratch_r2.resize(2*bod->getNumPosVars() + 8*bod->getNumDofs()); + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); //convenience btScalar *pMem = &scratch_r2[0]; - btScalar *scratch_q0 = pMem; pMem += bod->getNumPosVars(); - btScalar *scratch_qx = pMem; pMem += bod->getNumPosVars(); - btScalar *scratch_qd0 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qd1 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qd2 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qd3 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qdd0 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qdd1 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qdd2 = pMem; pMem += bod->getNumDofs(); - btScalar *scratch_qdd3 = pMem; pMem += bod->getNumDofs(); - btAssert((pMem - (2*bod->getNumPosVars() + 8*bod->getNumDofs())) == &scratch_r2[0]); + btScalar *scratch_q0 = pMem; pMem += numPosVars; + btScalar *scratch_qx = pMem; pMem += numPosVars; + btScalar *scratch_qd0 = pMem; pMem += numDofs; + btScalar *scratch_qd1 = pMem; pMem += numDofs; + btScalar *scratch_qd2 = pMem; pMem += numDofs; + btScalar *scratch_qd3 = pMem; pMem += numDofs; + btScalar *scratch_qdd0 = pMem; pMem += numDofs; + btScalar *scratch_qdd1 = pMem; pMem += numDofs; + btScalar *scratch_qdd2 = pMem; pMem += numDofs; + btScalar *scratch_qdd3 = pMem; pMem += numDofs; + btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); + ///// //copy q0 to scratch_q0 and qd0 to scratch_qd0 + scratch_q0[0] = bod->getWorldToBaseRot().x(); + scratch_q0[1] = bod->getWorldToBaseRot().y(); + scratch_q0[2] = bod->getWorldToBaseRot().z(); + scratch_q0[3] = bod->getWorldToBaseRot().w(); + scratch_q0[4] = bod->getBasePos().x(); + scratch_q0[5] = bod->getBasePos().y(); + scratch_q0[6] = bod->getBasePos().z(); + // for(int link = 0; link < bod->getNumLinks(); ++link) { for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - - for(int dof = 0; dof < bod->getLink(link).m_dofCount; ++dof) - scratch_qd0[bod->getLink(link).m_dofOffset + dof] = bod->getVelocityVector()[6 + bod->getLink(link).m_dofOffset + dof]; + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; } + // + for(int dof = 0; dof < numDofs; ++dof) + scratch_qd0[dof] = bod->getVelocityVector()[dof]; + //// auto pResetQx = [&scratch_qx, &scratch_q0, &bod]() { - for(int dof = 0; dof < bod->getNumPosVars(); ++dof) + for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) scratch_qx[dof] = scratch_q0[dof]; }; @@ -509,12 +521,12 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) pVal[i] = pCurVal[i] + dt * pDer[i]; }; // - auto pCopyToVelocitVector = [](btMultiBody *pBody, const btScalar *pData) + auto pCopyToVelocityVector = [](btMultiBody *pBody, const btScalar *pData) { btScalar *pVel = const_cast(pBody->getVelocityVector()); - for(int i = 0; i < pBody->getNumDofs(); ++i) - pVel[6+i] = pData[i]; + for(int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; }; // auto pCopy = [](const btScalar *pSrc, btScalar *pDst, int start, int size) @@ -524,72 +536,77 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) }; // - btScalar h = solverInfo.m_timeStep; - //calc qdd0 from: q0 & qd0 + btScalar h = solverInfo.m_timeStep; + #define output &scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd0, bod->getNumDofs() + 6, bod->getNumDofs()); + pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, bod->getNumDofs()); + pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); // - //calc qdd1 from: q1 & qd1 - pCopyToVelocitVector(bod, scratch_qd1); + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd1, bod->getNumDofs() + 6, bod->getNumDofs()); + pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, bod->getNumDofs()); + pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); // //calc qdd2 from: q2 & qd2 - pCopyToVelocitVector(bod, scratch_qd2); + pCopyToVelocityVector(bod, scratch_qd2); bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd2, bod->getNumDofs() + 6, bod->getNumDofs()); + pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, bod->getNumDofs()); + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); // //calc qdd3 from: q3 & qd3 - pCopyToVelocitVector(bod, scratch_qd3); + pCopyToVelocityVector(bod, scratch_qd3); bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(&scratch_r[0], scratch_qdd3, bod->getNumDofs() + 6, bod->getNumDofs()); + pCopy(output, scratch_qdd3, 0, numDofs); // //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - //btAlignedObjectArray res_qd; res_qd.resize(bod->getNumDofs()); - btAlignedObjectArray delta_q; delta_q.resize(bod->getNumDofs()); - btAlignedObjectArray delta_qd; delta_qd.resize(6+bod->getNumDofs()); - for(int i = 0; i < bod->getNumDofs(); ++i) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray delta_q; delta_q.resize(numDofs); + btAlignedObjectArray delta_qd; delta_qd.resize(numDofs); + for(int i = 0; i < numDofs; ++i) { - delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); - delta_qd[6+i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); + delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[6+i] = h*scratch_qdd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; } - + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); // if(!doNotUpdatePos) { btScalar *pRealBuf = const_cast(bod->getVelocityVector()); pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - for(int i = 0; i < bod->getNumPosVars(); ++i) + for(int i = 0; i < numDofs; ++i) pRealBuf[i] = delta_q[i]; //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->__posUpdated = true; + bod->__posUpdated = true; } - // - pCopyToVelocitVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - //delete [] scratch_q; + + //ugly hack which resets the cached data to t0 (needed for constraint solver) + { + for(int link = 0; link < bod->getNumLinks(); ++link) + bod->getLink(link).updateCacheMultiDof(); + bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); + } + } } else From c0530d31ec4dc32db99a895f260a11ef40bbf6b9 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:02:11 +0100 Subject: [PATCH 07/17] minor naming chamge --- .../Featherstone/btMultiBody.cpp | 40 ++++++++++--------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index e0dac8c3a..9179ed1b1 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2602,7 +2602,7 @@ void btMultiBody::fillContactJacobianMultiDof(int link, scratch_m.resize(num_links + 1); btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1; + btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; btVector3 * n_local = v_ptr; v_ptr += num_links + 1; btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); @@ -2612,22 +2612,24 @@ void btMultiBody::fillContactJacobianMultiDof(int link, btMatrix3x3 * rot_from_world = &scratch_m[0]; const btVector3 p_minus_com_world = contact_point - m_basePos; + const btVector3 &normal_world = normal; //convenience - rot_from_world[0] = btMatrix3x3(m_baseQuat); - - p_minus_com[0] = rot_from_world[0] * p_minus_com_world; - n_local[0] = rot_from_world[0] * normal; + rot_from_world[0] = btMatrix3x3(m_baseQuat); // omega coeffients first. - btVector3 omega_coeffs; - omega_coeffs = p_minus_com_world.cross(normal); - jac[0] = omega_coeffs[0]; - jac[1] = omega_coeffs[1]; - jac[2] = omega_coeffs[2]; + btVector3 omega_coeffs_world; + omega_coeffs_world = p_minus_com_world.cross(normal_world); + jac[0] = omega_coeffs_world[0]; + jac[1] = omega_coeffs_world[1]; + jac[2] = omega_coeffs_world[2]; // then v coefficients - jac[3] = normal[0]; - jac[4] = normal[1]; - jac[5] = normal[2]; + jac[3] = normal_world[0]; + jac[4] = normal_world[1]; + jac[5] = normal_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[0] = rot_from_world[0] * normal_world; // Set remaining jac values to zero for now. for (int i = 6; i < 6 + m_dofCount; ++i) @@ -2651,14 +2653,14 @@ void btMultiBody::fillContactJacobianMultiDof(int link, rot_from_world[i+1] = mtx * rot_from_world[parent+1]; n_local[i+1] = mtx * n_local[parent+1]; - p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector; + 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[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); break; } case btMultibodyLink::ePrismatic: @@ -2668,16 +2670,16 @@ void btMultiBody::fillContactJacobianMultiDof(int link, } case btMultibodyLink::eSpherical: { - results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(2)); + 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].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(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[i+1]));// + m_links[i].getAxisBottom(0)); + 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)); From 81447aa7c54db0c9a87413d9e752a08c7bbabbae Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:03:20 +0100 Subject: [PATCH 08/17] dirty commit: experimenting with the 6DoF grabbing/p2p constraint --- .../Featherstone/btMultiBody.cpp | 129 +++++++++++++++ src/BulletDynamics/Featherstone/btMultiBody.h | 9 ++ .../Featherstone/btMultiBodyConstraint.cpp | 5 + .../Featherstone/btMultiBodyPoint2Point.cpp | 151 ++++++++++++++++++ .../Featherstone/btMultiBodyPoint2Point.h | 2 + 5 files changed, 296 insertions(+) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 9179ed1b1..490ebd409 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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 &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &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, diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 41cb783d3..cc86cc4c8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -397,6 +397,15 @@ public: btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m) const; + void fillContactJacobianMultiDof_test(int link, + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const; + // // sleeping diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 609f92d63..d3045fd3e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 029f33449..d4fa1d43b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -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;igetNumLinks();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;igetNumLinks();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 \ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index 824a57393..cae8fa917 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -20,6 +20,8 @@ subject to the following restrictions: #include "btMultiBodyConstraint.h" +#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + class btMultiBodyPoint2Point : public btMultiBodyConstraint { protected: From 87a98939ebd46500a45395387f0a2e392b7575a2 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:04:39 +0100 Subject: [PATCH 09/17] tighter packing in btMultiBodySolverConstraint --- .../Featherstone/btMultiBodySolverConstraint.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index f48935d74..45e31a176 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -32,14 +32,15 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint {} int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; int m_jacAindex; - int m_deltaVelBindex; + int m_jacBindex; + + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; btVector3 m_relpos2CrossNormal; btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - int m_jacBindex; + btVector3 m_angularComponentA; btVector3 m_angularComponentB; From eb66b22034cd2c9ee882e1ecfe7950843ae4e6fb Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:06:58 +0100 Subject: [PATCH 10/17] dirty commit: starting to unify btMultiBoydConstraint::fillMultiBodyConstraint..(..) --- .../Featherstone/btMultiBodyConstraint.cpp | 358 +++++++++++++++++- .../Featherstone/btMultiBodyConstraint.h | 12 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 4 +- .../Featherstone/btMultiBodyPoint2Point.h | 2 +- 4 files changed, 369 insertions(+), 7 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d3045fd3e..782a1d28d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -242,7 +242,7 @@ void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala } -void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, +void btMultiBodyConstraint::fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, const btVector3& contactNormalOnB, const btVector3& posAworld, const btVector3& posBworld, @@ -251,8 +251,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - - + btVector3 rel_pos1 = posAworld; btVector3 rel_pos2 = posBworld; @@ -483,6 +482,359 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr } + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + */ + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + //commented out on purpose, see below + //if (penetration>0) + //{ + // positionalError = 0; + // velocityError = -penetration / infoGlobal.m_timeStep; + + //} else + //{ + // positionalError = -penetration * erp/infoGlobal.m_timeStep; + //} + + //we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints) + //both are valid in general and definitely so in the case of a point2Point constraint + positionalError = -penetration * erp/infoGlobal.m_timeStep; + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; + solverConstraint.m_upperLimit = m_maxAppliedImpulse; + } + +} + +void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar position, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + + btVector3 rel_pos1 = posAworld; + btVector3 rel_pos2 = posBworld; + + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = posAworld; + const btVector3& pos2 = posBworld; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + //determine jacobian of this 1D constraint + //resize.. + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + //copy/determine + if(jacOrgA) + { + for (int i=0;iisMultiDof()) + 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); + } + + //determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint) + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + 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); + else + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } + else if(rb0) + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + //determine jacobian of this 1D constraint + //resize.. + solverConstraint.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + //copy/determine.. + if(jacOrgB) + { + for (int i=0;iisMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + //determine.. + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + + } + else if(rb1) + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormalOnB; + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + } + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else if(rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + if (multiBodyB) + { + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + else if(rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + + solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + + + restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } + + ///warm starting (or zero if disabled) /* if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 12b5a33f2..7b27df1c5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -66,7 +66,7 @@ protected: void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, + void fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, const btVector3& contactNormalOnB, const btVector3& posAworld, const btVector3& posBworld, @@ -83,6 +83,16 @@ protected: btScalar lowerLimit, btScalar upperLimit); + void fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar position, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + public: btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index d4fa1d43b..13336ae56 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -131,7 +131,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co } btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); btScalar relaxation = 1.f; - fillMultiBodyConstraintMixed(constraintRow, data, + fillMultiBodyConstraintMixed_old(constraintRow, data, contactNormalOnB, pivotAworld, pivotBworld, position, @@ -278,7 +278,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co m_bodyA->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - fillMultiBodyConstraintMixed(constraintRow, data, + fillMultiBodyConstraintMixed_old(constraintRow, data, contactNormalOnB, pivotAworld, pivotBworld, position, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index cae8fa917..c9bfbc176 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -20,7 +20,7 @@ subject to the following restrictions: #include "btMultiBodyConstraint.h" -#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST +//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST class btMultiBodyPoint2Point : public btMultiBodyConstraint { From 0ba7d69f8650e837d951b764760dbf3fb1f6806c Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:07:53 +0100 Subject: [PATCH 11/17] fixed a jacobian sizing bug (m_jacSizeBoth) --- src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 782a1d28d..9f0b0b9b4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -28,7 +28,9 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); else m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); - } + } + else + m_jacSizeBoth = m_jacSizeA; m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); m_data.resize((2 + m_jacSizeBoth) * m_numRows); From ef6abf64903df24e5930243627626ceca8c80cbd Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:09:44 +0100 Subject: [PATCH 12/17] unified btMultiBodyConstrained::fillMultiBodyConstraint..(...) mtds + cleaned some of the earlier dirty changes (6DoF grabbing constraint stuff mainly) --- .../Featherstone/btMultiBody.cpp | 121 +--- src/BulletDynamics/Featherstone/btMultiBody.h | 7 +- .../Featherstone/btMultiBodyConstraint.cpp | 676 ++---------------- .../Featherstone/btMultiBodyConstraint.h | 26 +- .../btMultiBodyJointLimitConstraint.cpp | 4 +- .../Featherstone/btMultiBodyJointMotor.cpp | 4 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 341 ++++----- 7 files changed, 254 insertions(+), 925 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 490ebd409..f0f34b506 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2587,126 +2587,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } } -void btMultiBody::fillContactJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const -{ - // temporary space - int num_links = getNumLinks(); - int m_dofCount = getNumDofs(); - scratch_v.resize(2*num_links + 2); - 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 = 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_world = normal; //convenience - - rot_from_world[0] = btMatrix3x3(m_baseQuat); - - // omega coeffients first. - btVector3 omega_coeffs_world; - omega_coeffs_world = p_minus_com_world.cross(normal_world); - jac[0] = omega_coeffs_world[0]; - jac[1] = omega_coeffs_world[1]; - jac[2] = omega_coeffs_world[2]; - // then v coefficients - jac[3] = normal_world[0]; - jac[4] = normal_world[1]; - jac[5] = normal_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[0] = rot_from_world[0] * normal_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[i+1] = mtx * n_local[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[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - break; - } - case btMultibodyLink::ePrismatic: - { - results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisBottom(0)); - break; - } - case btMultibodyLink::eSpherical: - { - 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].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(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::fillContactJacobianMultiDof_test(int link, +void btMultiBody::filConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index cc86cc4c8..4c99aea4c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -389,15 +389,18 @@ public: btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m) const; + //multidof version of fillContactJacobian void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const; + btAlignedObjectArray &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } - void fillContactJacobianMultiDof_test(int link, + //a more general version of fillContactJacobianMultiDof which does not assume.. + //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only + void filConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 9f0b0b9b4..1bb518f94 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -40,579 +40,42 @@ btMultiBodyConstraint::~btMultiBodyConstraint() { } - - -btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit) -{ - - - - constraintRow.m_multiBodyA = m_bodyA; - constraintRow.m_multiBodyB = m_bodyB; - - btMultiBody* multiBodyA = constraintRow.m_multiBodyA; - btMultiBody* multiBodyB = constraintRow.m_multiBodyB; - - if (multiBodyA) - { - - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; //total dof count of tree A - - constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (constraintRow.m_deltaVelAindex <0) //if this multibody does not have a place allocated in m_deltaVelocities... - { - constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); //=> each constrained tree's dofs are represented in m_deltaVelocities - } else - { - btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); - } - - constraintRow.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - for (int i=0;iisMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (constraintRow.m_deltaVelBindex <0) - { - constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - constraintRow.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - - for (int i=0;iisMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &data.m_jacobians[constraintRow.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &data.m_jacobians[constraintRow.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - constraintRow.m_jacDiagABInv = 1.f/(d); - } else - { - constraintRow.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining constraintRow fields - - - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - for (int i = 6; i < ndofA ; ++i) - printf("%.4f ", multiBodyA->getVelocityVector()[i]); - printf("\nrel_vel = %.4f\n------------\n", rel_vel); - constraintRow.m_friction = 0.f; - - constraintRow.m_appliedImpulse = 0.f; - constraintRow.m_appliedPushImpulse = 0.f; - - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - btScalar erp = infoGlobal.m_erp2; - - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse) - { - //combine position and velocity into rhs - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - } - - - constraintRow.m_cfm = 0.f; - constraintRow.m_lowerLimit = lowerLimit; - constraintRow.m_upperLimit = upperLimit; - - } - return rel_vel; -} - - void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { for (int i = 0; i < ndof; ++i) data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } - -void btMultiBodyConstraint::fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar position, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - - btVector3 rel_pos1 = posAworld; - btVector3 rel_pos2 = posBworld; - - solverConstraint.m_multiBodyA = m_bodyA; - solverConstraint.m_multiBodyB = m_bodyB; - solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; - - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - const btVector3& pos1 = posAworld; - const btVector3& pos2 = posBworld; - - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); - btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - if (multiBodyA) - { - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - 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); - else - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } else - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - solverConstraint.m_jacBindex = data.m_jacobians.size(); - - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - - if(multiBodyB->isMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); - } else - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - } - - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } else - { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - } - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } else - { - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else - { - if (rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else - { - if (rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - - - restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - //commented out on purpose, see below - //if (penetration>0) - //{ - // positionalError = 0; - // velocityError = -penetration / infoGlobal.m_timeStep; - - //} else - //{ - // positionalError = -penetration * erp/infoGlobal.m_timeStep; - //} - - //we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints) - //both are valid in general and definitely so in the case of a point2Point constraint - positionalError = -penetration * erp/infoGlobal.m_timeStep; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; - solverConstraint.m_upperLimit = m_maxAppliedImpulse; - } - -} - -void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, +btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, btScalar* jacOrgA, btScalar* jacOrgB, const btVector3& contactNormalOnB, const btVector3& posAworld, const btVector3& posBworld, - btScalar position, + btScalar posError, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - - - btVector3 rel_pos1 = posAworld; - btVector3 rel_pos2 = posBworld; - solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; - + solverConstraint.m_linkB = m_linkB; btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - const btVector3& pos1 = posAworld; - const btVector3& pos2 = posBworld; - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); if (multiBodyA) { @@ -630,7 +93,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); } - //determine jacobian of this 1D constraint + //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom //resize.. solverConstraint.m_jacAindex = data.m_jacobians.size(); data.m_jacobians.resize(data.m_jacobians.size()+ndofA); @@ -649,11 +112,12 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); } - //determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint) + //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - + //determine.. if(multiBodyA->isMultiDof()) multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); else @@ -679,7 +143,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); } - //determine jacobian of this 1D constraint + //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom //resize.. solverConstraint.m_jacBindex = data.m_jacobians.size(); data.m_jacobians.resize(data.m_jacobians.size()+ndofB); @@ -697,7 +161,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); } - //determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint) + //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) //resize.. data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); @@ -716,7 +180,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormalOnB; } - { btVector3 vec; @@ -724,78 +187,72 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint btScalar denom1 = 0.f; btScalar* jacB = 0; btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 0; int ndofA = 0; + //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) if (multiBodyA) { ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; + btScalar l = deltaVelA[i]; denom0 += j*l; } - } else - { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } } + else if(rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + // if (multiBodyB) { const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; + btScalar l = deltaVelB[i]; denom1 += j*l; } - } else + } + else if(rb1) { - if (rb1) + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + } + //determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + denom1 += jacB[i] * deltaVelA[i]; + denom1 += jacA[i] * deltaVelB[i]; } } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { + // + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { solverConstraint.m_jacDiagABInv = 1.f; - } - + } } //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; + btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; btScalar rel_vel = 0.f; int ndofA = 0; @@ -827,13 +284,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint } solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - - - restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; } @@ -870,17 +320,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint } } else */ - { - solverConstraint.m_appliedImpulse = 0.f; - } + solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; - { - + { btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; + btScalar velocityError = desiredVelocity - rel_vel;// * damping; btScalar erp = infoGlobal.m_erp2; @@ -889,19 +336,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint erp = infoGlobal.m_erp; } - //commented out on purpose, see below - //if (penetration>0) - //{ - // positionalError = 0; - // velocityError = -penetration / infoGlobal.m_timeStep; - - //} else - //{ - // positionalError = -penetration * erp/infoGlobal.m_timeStep; - //} - - //we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints) - //both are valid in general and definitely so in the case of a point2Point constraint positionalError = -penetration * erp/infoGlobal.m_timeStep; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; @@ -921,8 +355,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint } solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; - solverConstraint.m_upperLimit = m_maxAppliedImpulse; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; } + return rel_vel; + } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 7b27df1c5..d01870485 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -66,32 +66,16 @@ protected: void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - void fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar position, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit); - - void fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, btScalar* jacOrgA, btScalar* jacOrgB, const btVector3& contactNormalOnB, const btVector3& posAworld, const btVector3& posBworld, - btScalar position, + btScalar posError, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation = 1.f, + bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); public: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 21baf3d0e..b9658731b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -99,8 +99,10 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; + const btScalar posError = 0; //why assume it's zero? + const btVector3 dummy(0, 0, 0); - btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,m_maxAppliedImpulse); + btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); { btScalar penetration = getPosition(row); btScalar positionalError = 0.f; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 527f2c011..2f10118a0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -82,13 +82,15 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con // directions were set in the ctor and never change. + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); for (int row=0;rowgetIslandTag(); - - if (m_bodyA) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();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;igetNumLinks();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<3;i++) - { - - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - - btVector3 contactNormalOnB(0,0,0); - contactNormalOnB[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; - fillMultiBodyConstraintMixed_old(constraintRow, data, - contactNormalOnB, - pivotAworld, pivotBworld, - position, - infoGlobal, - relaxation, - false); - constraintRow.m_lowerLimit = -m_maxAppliedImpulse; - constraintRow.m_upperLimit = m_maxAppliedImpulse; - - } -} - + #define BTMBP2PCONSTRAINT_DIM 3 #else - - - -#include "btMultiBodyPoint2Point.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" + #define BTMBP2PCONSTRAINT_DIM 6 +#endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,6,false), + :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -162,7 +35,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,6,false), + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), @@ -223,7 +96,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co { // int i=1; - for (int i=0;i<6;i++) + for (int i=0;i= 3) - contactNormalOnB[i-3] = -1; - else - normalAng[i] = -1; - +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + contactNormalOnB[i] = -1; +#else + contactNormalOnB[i%3] = -1; +#endif btScalar penetration = 0; @@ -265,30 +136,180 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co 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]; + btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; - m_bodyA->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - - fillMultiBodyConstraintMixed_old(constraintRow, data, - contactNormalOnB, - pivotAworld, pivotBworld, - position, - infoGlobal, - relaxation, - false); - constraintRow.m_lowerLimit = -m_maxAppliedImpulse; - constraintRow.m_upperLimit = m_maxAppliedImpulse; + fillMultiBodyConstraint(constraintRow, data, 0, 0, + contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); +#else + const btVector3 dummy(0, 0, 0); + + btAssert(m_bodyA->isMultiDof()); + + btScalar* jac1 = jacobianA(i); + const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; + const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; + + m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + + fillMultiBodyConstraint(constraintRow, data, jac1, 0, + dummy, dummy, dummy, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); +#endif } } -#endif \ No newline at end of file + + +//#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;igetNumLinks();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;igetNumLinks();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->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); +// +// +// fillMultiBodyConstraintMixed_old(constraintRow, data, +// contactNormalOnB, +// pivotAworld, pivotBworld, +// position, +// infoGlobal, +// relaxation, +// false); +// constraintRow.m_lowerLimit = -m_maxAppliedImpulse; +// constraintRow.m_upperLimit = m_maxAppliedImpulse; +// +// } +//} \ No newline at end of file From 736ba01423127dd3c03a7f28c8be64c27198cfb7 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:10:45 +0100 Subject: [PATCH 13/17] minor clean-up --- .../Featherstone/btMultiBodyPoint2Point.cpp | 147 +----------------- 1 file changed, 1 insertion(+), 146 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 73beb03c7..84f6f39fe 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -167,149 +167,4 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co ); #endif } -} - - - -//#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;igetNumLinks();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;igetNumLinks();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->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); -// -// -// fillMultiBodyConstraintMixed_old(constraintRow, data, -// contactNormalOnB, -// pivotAworld, pivotBworld, -// position, -// infoGlobal, -// relaxation, -// false); -// constraintRow.m_lowerLimit = -m_maxAppliedImpulse; -// constraintRow.m_upperLimit = m_maxAppliedImpulse; -// -// } -//} \ No newline at end of file +} \ No newline at end of file From cbf2d915d177c3e774e607d8a8559c0065d221fc Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:12:02 +0100 Subject: [PATCH 14/17] fixed the multibody jitter issue + several friction-related fixes --- .../btMultiBodyConstraintSolver.cpp | 61 ++++++++++++------- 1 file changed, 38 insertions(+), 23 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 1e4004af3..81a30f448 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -159,6 +159,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyA) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity if(c.m_multiBodyA->isMultiDof()) c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); else @@ -171,6 +173,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyB) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity if(c.m_multiBodyB->isMultiDof()) c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); else @@ -458,12 +462,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_friction = cp.m_combinedFriction; - - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) + if(!isFriction) { - restitution = 0.f; - }; + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } } @@ -511,11 +517,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - + btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) @@ -526,7 +530,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (penetration>0) { positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; + velocityError -= penetration / infoGlobal.m_timeStep; } else { @@ -536,22 +540,33 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + if(!isFriction) { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; - } else + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + else { - //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; } - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; + solverConstraint.m_cfm = 0.f; //why not use cfmSlip? } } @@ -734,6 +749,10 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* { btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); @@ -741,10 +760,6 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); } - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { cp.m_lateralFrictionInitialized = true; From 2cbcd86de95745402af997585d2b5a6cde46bdb5 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:13:22 +0100 Subject: [PATCH 15/17] fixed a btMultiBody ctor bug --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index f0f34b506..6b594d085 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -101,7 +101,7 @@ btMultiBody::btMultiBody(int n_links, m_useGyroTerm(true), m_maxAppliedImpulse(1000.f), m_hasSelfCollision(true), - m_dofCount(n_links), + m_dofCount(0), __posUpdated(false), m_isMultiDof(multiDof), m_posVarCnt(0) From aa87e47d2d76ac2523809fcedf119e32c6382cc3 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:14:48 +0100 Subject: [PATCH 16/17] preparing for stabilization investigation: useRK4 is now a btMultiBody flag (not world's), reenabled global velocities (as a flag-controlled option), made the test application easier to handle for multiple multibodiez and added a max coordinate multibody (created from btMultiBody) --- .../Featherstone/btMultiBody.cpp | 60 +++++++++---------- src/BulletDynamics/Featherstone/btMultiBody.h | 7 +++ .../Featherstone/btMultiBodyDynamicsWorld.cpp | 4 +- .../Featherstone/btMultiBodyDynamicsWorld.h | 4 -- 4 files changed, 39 insertions(+), 36 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 6b594d085..91b575bed 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -104,7 +104,8 @@ btMultiBody::btMultiBody(int n_links, m_dofCount(0), __posUpdated(false), m_isMultiDof(multiDof), - m_posVarCnt(0) + m_posVarCnt(0), + m_useRK4(false), m_useGlobalVelocities(false) { if(!m_isMultiDof) @@ -1155,7 +1156,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, rot_from_world[0] = rot_from_parent[0]; // - bool useGlobalVelocities = false; for (int i = 0; i < num_links; ++i) { const int parent = m_links[i].m_parent; rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); @@ -1201,7 +1201,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // now set vhat_i to its true value by doing // vhat_i += qidot * shat_i - if(!useGlobalVelocities) + if(!m_useGlobalVelocities) { spatJointVel.setZero(); @@ -1494,37 +1494,37 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, ///// ///////////////////// - //if(useGlobalVelocities) - //{ - // for (int i = 0; i < num_links; ++i) - // { - // const int parent = m_links[i].m_parent; - // //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done - // //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done - // - // fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - // fromWorld.m_rotMat = rot_from_world[i+1]; - // - // // vhat_i = i_xhat_p(i) * vhat_p(i) - // fromParent.transform(spatVel[parent+1], spatVel[i+1]); - // //nice alternative below (using operator *) but it generates temps - // ///////////////////////////////////////////////////////////// + if(m_useGlobalVelocities) + { + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done + //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + ///////////////////////////////////////////////////////////// - // // now set vhat_i to its true value by doing - // // vhat_i += qidot * shat_i - // spatJointVel.setZero(); + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + spatJointVel.setZero(); - // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - // spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - // - // // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - // spatVel[i+1] += spatJointVel; + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; - // fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); - // fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); - // } - //} + fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); + } + } } #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 4c99aea4c..8f6b0b851 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -489,6 +489,11 @@ public: bool isMultiDof() { return m_isMultiDof; } void finalizeMultiDof(); + void useRK4Integration(bool use) { m_useRK4 = use; } + bool isUsingRK4Integration() const { return m_useRK4; } + void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } + bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } + bool __posUpdated; private: @@ -530,6 +535,8 @@ private: btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; int m_dofCount, m_posVarCnt; + + bool m_useRK4, m_useGlobalVelocities; // // realBuf: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 9c2e0387d..90bd896a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -381,7 +381,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver), m_useRK4ForMultiBodies(false) + m_multiBodyConstraintSolver(constraintSolver) { //split impulse is not yet supported for Featherstone hierarchies getSolverInfo().m_splitImpulse = false; @@ -467,7 +467,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if(bod->isMultiDof()) { - if(!m_useRK4ForMultiBodies) + if(!bod->isUsingRK4Integration()) bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 590906e16..a4d1a1161 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -34,7 +34,6 @@ protected: btAlignedObjectArray m_sortedMultiBodyConstraints; btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - bool m_useRK4ForMultiBodies; virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); @@ -53,8 +52,5 @@ public: virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); - - void useRK4IntegrationForMultiBodies(bool use) { m_useRK4ForMultiBodies = use; } - bool isUsingRK4IntegrationForMultiBodies() const { return m_useRK4ForMultiBodies; } }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H From 876293ac95d06b5b522de1d478ee12c81aa09f6d Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:15:51 +0100 Subject: [PATCH 17/17] minor: replaced convenience lambda functions --- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 54 ++++++++++++------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 90bd896a5..58e48cd9a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -508,32 +508,48 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) for(int dof = 0; dof < numDofs; ++dof) scratch_qd0[dof] = bod->getVelocityVector()[dof]; //// - - auto pResetQx = [&scratch_qx, &scratch_q0, &bod]() + struct { - for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - }; + btMultiBody *bod; + btScalar *scratch_qx, *scratch_q0; - auto pEulerIntegrate = [](btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) - { - for(int i = 0; i < size; ++i) - pVal[i] = pCurVal[i] + dt * pDer[i]; - }; + void operator()() + { + for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } + } pResetQx = {bod, scratch_qx, scratch_q0}; // - auto pCopyToVelocityVector = [](btMultiBody *pBody, const btScalar *pData) + struct { - btScalar *pVel = const_cast(pBody->getVelocityVector()); + void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) + { + for(int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + } - for(int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - }; + } pEulerIntegrate; // - auto pCopy = [](const btScalar *pSrc, btScalar *pDst, int start, int size) + struct + { + void operator()(btMultiBody *pBody, const btScalar *pData) + { + btScalar *pVel = const_cast(pBody->getVelocityVector()); + + for(int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + + } + } pCopyToVelocityVector; + // + struct { - for(int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - }; + void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) + { + for(int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } + } pCopy; // btScalar h = solverInfo.m_timeStep;