From 9baee29300632aa1bd4c5bcba798263ba3443824 Mon Sep 17 00:00:00 2001 From: jingyuc Date: Mon, 26 Jul 2021 11:28:37 -0400 Subject: [PATCH] simple rigid translation is working. --- build_cmake_pybullet_double.sh | 2 +- examples/ReducedDeformableDemo/BasicTest.cpp | 9 ++- .../btReducedSoftBody.cpp | 77 ++++++++++++++++++- .../BulletReducedSoftBody/btReducedSoftBody.h | 75 ++++++++++++++++-- .../btReducedSoftBodySolver.cpp | 60 +++++++++++++-- .../btReducedSoftBodySolver.h | 2 + src/BulletSoftBody/btDeformableBodySolver.cpp | 1 - 7 files changed, 208 insertions(+), 18 deletions(-) diff --git a/build_cmake_pybullet_double.sh b/build_cmake_pybullet_double.sh index 8817311f0..92680d734 100755 --- a/build_cmake_pybullet_double.sh +++ b/build_cmake_pybullet_double.sh @@ -5,7 +5,7 @@ if [ -e CMakeCache.txt ]; then fi mkdir -p build_cmake cd build_cmake -cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=ON -DUSE_DOUBLE_PRECISION=ON -DBT_USE_EGL=ON -DCMAKE_BUILD_TYPE=Release .. || exit 1 +cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=ON -DUSE_DOUBLE_PRECISION=ON -DBT_USE_EGL=ON -DCMAKE_BUILD_TYPE=Debug .. || exit 1 make -j $(command nproc 2>/dev/null || echo 12) || exit 1 cd examples cd pybullet diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index a829314ca..e3f2356b6 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -68,6 +68,8 @@ class BasicTest : public CommonDeformableBodyBase btAssert(rsb->m_nodes.size() == m_nFull); btAlignedObjectArray delta_x; delta_x.resize(m_nFull); + btVector3 origin = rsb->getWorldTransform().getOrigin(); + for (int i = 0; i < m_nFull; ++i) { for (int k = 0; k < 3; ++k) @@ -77,7 +79,7 @@ class BasicTest : public CommonDeformableBodyBase for (int j = 0; j < m_nReduced; ++j) delta_x[i][k] += rsb->m_modes[j][3 * i + k] * rsb->m_reducedDofs[j]; // get new coordinates - rsb->m_nodes[i].m_x[k] = rsb->m_x0[3 * i + k] + delta_x[i][k]; + rsb->m_nodes[i].m_x[k] = rsb->m_x0[3 * i + k] + delta_x[i][k] + origin[k]; //TODO: assume the initial origin is at (0,0,0) } } @@ -240,8 +242,13 @@ void BasicTest::initPhysics() btAlignedObjectArray mass_array; btReducedSoftBodyHelpers::readBinary(mass_array, 0, 3 * m_nFull, 3 * m_nFull, M_file.c_str()); // assign mass to nodes + btScalar mass = 0; for (int i = 0; i < rsb->m_nodes.size(); ++i) + { rsb->m_nodes[i].m_im = mass_array[3 * i]; // here we use m_im as the actual mass not the mass inverse + mass += mass_array[3 * i]; + } + rsb->setMass(mass); rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 6f7f0cecb..f56ef7b30 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -1,8 +1,83 @@ #include "btReducedSoftBody.h" +#include "LinearMath/btTransformUtil.h" btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) : btSoftBody(worldInfo, node_count, x, m) { - // other members + // model flag m_reducedModel = true; + + // rigid motion + m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_angularVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + m_angularFactor.setValue(1, 1, 1); + m_linearFactor.setValue(1, 1, 1); + m_invInertiaLocal.setValue(1, 1, 1); //TODO: get correct inertia through shape + m_mass = 0.0; + m_inverseMass = 0.0; +} + +void btReducedSoftBody::setMass(btScalar m) +{ + m_mass = m; + m_inverseMass = m > 0 ? 1.0 / m : 0; +} + +void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) +{ + btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); +} + +void btReducedSoftBody::proceedToTransform(const btTransform& newTrans) +{ + setCenterOfMassTransform(newTrans); +} + +void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform) +{ + if (isKinematicObject()) + { + m_interpolationWorldTransform = m_worldTransform; + } + else + { + m_interpolationWorldTransform = xform; + } + m_interpolationLinearVelocity = getLinearVelocity(); + m_interpolationAngularVelocity = getAngularVelocity(); + m_worldTransform = xform; + updateInertiaTensor(); +} + +void btReducedSoftBody::updateInertiaTensor() +{ + m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); +} + +void btReducedSoftBody::applyCentralImpulse(const btVector3& impulse) +{ + m_linearVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif +} + +void btReducedSoftBody::applyTorqueImpulse(const btVector3& torque) +{ + m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif +} + +void btReducedSoftBody::applyImpulse(const btVector3& impulse, const btVector3& rel_pos) +{ + if (m_inverseMass != btScalar(0.)) + { + applyCentralImpulse(impulse); + if (m_angularFactor) + { + applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor)); + } + } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 38adbae7f..357db6936 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -1,15 +1,22 @@ #ifndef BT_REDUCED_SOFT_BODY_H #define BT_REDUCED_SOFT_BODY_H +#include "../btSoftBody.h" #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btVector3.h" - -// #include "BulletDynamics/Dynamics/btRigidBody.h" -#include "../btSoftBody.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btTransform.h" // Reduced deformable body is a simplified deformable object embedded in a rigid frame. class btReducedSoftBody : public btSoftBody { + private: + // + + protected: + // + + public: // // Typedefs @@ -18,8 +25,6 @@ class btReducedSoftBody : public btSoftBody typedef btAlignedObjectArray tDenseArray; typedef btAlignedObjectArray > tDenseMatrix; - using btSoftBody::tNodeArray; - // // Fields // @@ -33,20 +38,74 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model tDenseArray m_Kr; // reduced stiffness matrix tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this? + + // rigid frame + btScalar m_mass; + btScalar m_inverseMass; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btVector3 m_linearFactor; + btVector3 m_angularFactor; + btVector3 m_invInertiaLocal; + btMatrix3x3 m_invInertiaTensorWorld; // full space tDenseArray m_x0; // Rest position - // rigid frame - - // // Api // btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); ~btReducedSoftBody() {} + void setMass(btScalar m); + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + + // rigid motion related + + void applyCentralImpulse(const btVector3& impulse); + + void applyTorqueImpulse(const btVector3& torque); + + void applyImpulse(const btVector3& impulse, const btVector3& rel_pos); + + void proceedToTransform(const btTransform& newTrans); + + void setCenterOfMassTransform(const btTransform& xform); + + void updateInertiaTensor(); + + const btVector3& getLinearVelocity() const + { + return m_linearVelocity; + } + const btVector3& getAngularVelocity() const + { + return m_angularVelocity; + } + + const btVector3& getOrigin() const + { + return m_worldTransform.getOrigin(); + } + + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + void clampVelocity(btVector3& v) const { + v.setX( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getX())) + ); + v.setY( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getY())) + ); + v.setZ( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) + ); + } + #endif }; #endif // BT_REDUCED_SOFT_BODY_H \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 0853d1d00..8794c4b70 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -1,6 +1,19 @@ #include "btReducedSoftBodySolver.h" #include "../btDeformableMultiBodyDynamicsWorld.h" +void btReducedSoftBodySolver::predictMotion(btScalar solverdt) +{ + applyForce(); + + // apply rigid motion + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + + // rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); + } +} + void btReducedSoftBodySolver::applyForce() { for (int i = 0; i < m_softBodies.size(); ++i) @@ -28,17 +41,47 @@ void btReducedSoftBodySolver::applyForce() // apply impulses to reduced deformable objects static btScalar sim_time = 0; - static btScalar target_vel = 20; - static bool apply_impulse = true; - if (rsb->m_reducedModel && apply_impulse && sim_time > 1) + static btScalar target_vel = 5; + static int apply_impulse = 0; + if (rsb->m_reducedModel && apply_impulse < 4 && sim_time > 1) { - apply_impulse = false; - - btScalar f_imp = rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; + btScalar f_imp = 0; + btVector3 imp(0, 0, 0); + if (sim_time > 1 && apply_impulse == 0) + { + f_imp = rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; + imp[1] = f_imp; + apply_impulse++; + std::cout << "hit" << "\n"; + } + if (sim_time > 2 && apply_impulse == 1) + { + f_imp = - 1.2 * rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; + imp[1] = f_imp; + apply_impulse++; + std::cout << "hit2" << "\n"; + } + if (sim_time > 3 && apply_impulse == 2) + { + f_imp = 1 * rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; + imp[2] = f_imp; + apply_impulse++; + std::cout << "hit3" << "\n"; + } + if (sim_time > 4 && apply_impulse == 3) + { + f_imp = -1.1 * rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; + imp[2] = f_imp; + apply_impulse++; + std::cout << "hit4" << "\n"; + } for (int i = 0; i < rsb->m_reducedDofs.size(); ++i) { reduced_force[i] += rsb->m_modes[i][0 * 3 + 1] * f_imp; } + + + rsb->applyImpulse(imp, rsb->m_nodes[0].m_x); } // update reduced velocity @@ -66,5 +109,10 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) rsb->m_reducedDofs[r] += timeStep * rsb->m_reducedVelocity[r]; + + // rigid motion + rsb->predictIntegratedTransform(timeStep, rsb->getInterpolationWorldTransform()); + + rsb->proceedToTransform(rsb->getInterpolationWorldTransform()); } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index 15d68979d..7a1e53366 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -24,6 +24,8 @@ class btReducedSoftBodySolver : public btDeformableBodySolver // virtual void solveConstraints(btScalar solver_dt); + virtual void predictMotion(btScalar solverdt); + virtual void applyExplicitForce(); virtual void applyTransforms(btScalar timeStep); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index c91b84d24..68fbbe76f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -512,7 +512,6 @@ void btDeformableBodySolver::applyExplicitForce() void btDeformableBodySolver::applyTransforms(btScalar timeStep) { - std::cout << "called base\n"; for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i];