Fixed a bug in btReducedSoftBody::transform(): the mesh was rotated/translated incorrectly using btSoftBody::transform(). Now works for mesh whose CoM is not at (0,0,0)

data/reduced_beam/beam_mesh_origin.vtk

@@ -0,0 +1,83 @@
+#vtk DataFile Version 2.0
+I don't think this matters
+ASCII
+DATASET UNSTRUCTURED_GRID
+POINTS 28 double
+0.0000000000000000 0.0000000000000000 0.0000000000000000
+0.0000000000000000 0.5000000000000000 0.0000000000000000
+1.0000000000000000 0.0000000000000000 0.0000000000000000
+1.0000000000000000 0.5000000000000000 0.0000000000000000
+0.0000000000000000 0.0000000000000000 4.0000000000000000
+0.0000000000000000 0.5000000000000000 4.0000000000000000
+1.0000000000000000 0.0000000000000000 4.0000000000000000
+1.0000000000000000 0.5000000000000000 4.0000000000000000
+0.0000000000000000 0.0000000000000000 3.0000000000000000
+0.0000000000000000 0.0000000000000000 2.0000000000000000
+0.0000000000000000 0.0000000000000000 1.0000000000000000
+1.0000000000000000 0.0000000000000000 1.0000000000000000
+1.0000000000000000 0.0000000000000000 2.0000000000000000
+1.0000000000000000 0.0000000000000000 3.0000000000000000
+0.0000000000000000 0.5000000000000000 3.0000000000000000
+0.0000000000000000 0.5000000000000000 2.0000000000000000
+0.0000000000000000 0.5000000000000000 1.0000000000000000
+1.0000000000000000 0.5000000000000000 3.0000000000000000
+1.0000000000000000 0.5000000000000000 2.0000000000000000
+1.0000000000000000 0.5000000000000000 1.0000000000000000
+0.5000000000000000 0.0000000000000000 0.5000000000000000
+0.5000000000000000 0.0000000000000000 1.5000000000000000
+0.5000000000000000 0.0000000000000000 2.5000000000000000
+0.5000000000000000 0.0000000000000000 3.5000000000000000
+0.5000000000000000 0.5000000000000000 3.5000000000000000
+0.5000000000000000 0.5000000000000000 2.5000000000000000
+0.5000000000000000 0.5000000000000000 1.5000000000000000
+0.5000000000000000 0.5000000000000000 0.5000000000000000
+CELLS 48 192
+4 22 12 17 13
+4 20 26 21 11
+4 23 8 5 24
+4 11 27 26 19
+4 27 11 26 20
+4 17 22 24 25
+4 7 23 13 6
+4 22 17 24 13
+4 9 25 21 15
+4 24 22 8 14
+4 24 8 22 23
+4 25 9 21 22
+4 23 7 13 24
+4 15 10 16 26
+4 14 25 9 15
+4 25 14 9 22
+4 24 23 22 13
+4 15 10 21 9
+4 15 21 10 26
+4 24 25 22 14
+4 2 11 27 20
+4 7 5 23 6
+4 11 21 12 26
+4 20 1 0 16
+4 18 25 12 17
+4 18 12 25 26
+4 5 7 23 24
+4 26 21 25 15
+4 20 16 27 1
+4 21 25 22 12
+4 20 16 10 26
+4 20 27 16 26
+4 20 10 16 0
+4 11 12 19 26
+4 14 22 8 9
+4 8 23 5 4
+4 24 14 8 5
+4 11 2 27 3
+4 2 27 1 20
+4 12 18 19 26
+4 27 2 1 3
+4 24 13 17 7
+4 20 2 0 1
+4 5 23 6 4
+4 20 10 21 26
+4 12 22 17 25
+4 3 27 11 19
+4 21 26 25 12
+CELL_TYPES 10

data/reduced_torus/reduced_torus.urdf

@@ -10,6 +10,6 @@
     <friction value= "0.5"/>
     <damping_coefficient value="0.00001"/>
     <visual filename="torus_textured.obj"/>
-    <collision filename="torus_mesh_visual.vtk"/>
+    <collision filename="torus_mesh.vtk"/>
   </reduced_deformable>
 </robot>

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp

@@ -15,6 +15,7 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co
   m_nReduced = 0;
   m_nFull = 0;
 
+  m_transform_lock = false;
   m_ksScale = 1.0;
   m_rhoScale = 1.0;
 
@@ -80,11 +81,13 @@ void btReducedSoftBody::setMassProps(const tDenseArray& mass_array)
 void btReducedSoftBody::setInertiaProps()
 {
   // make sure the initial CoM is at the origin (0,0,0)
-  for (int i = 0; i < m_nFull; ++i)
-  {
-    m_nodes[i].m_x -= m_initialCoM;
-  }
-  m_initialCoM.setZero();
+  // for (int i = 0; i < m_nFull; ++i)
+  // {
+  //   m_nodes[i].m_x -= m_initialCoM;
+  // }
+  // m_initialCoM.setZero();
+  m_rigidTransformWorld.setOrigin(m_initialCoM);
+  m_interpolationWorldTransform = m_rigidTransformWorld;
   
   updateLocalInertiaTensorFromNodes();
 
@@ -399,17 +402,41 @@ void btReducedSoftBody::proceedToTransform(btScalar dt, bool end_of_time_step)
 
 void btReducedSoftBody::transformTo(const btTransform& trs)
 {
-  // get the current best rigid fit
 	btTransform current_transform = getRigidTransform();
-	// apply transform in material space
-	btTransform new_transform = trs * current_transform.inverse();
+	btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(),
+                            trs.getOrigin() - current_transform.getOrigin());
   transform(new_transform);
 }
 
 void btReducedSoftBody::transform(const btTransform& trs)
 {
-  // translate mesh
-	btSoftBody::transform(trs);
+  m_transform_lock = true;
+
+  // transform mesh
+  {
+    const btScalar margin = getCollisionShape()->getMargin();
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+
+    btVector3 CoM = m_rigidTransformWorld.getOrigin();
+    btVector3 translation = trs.getOrigin();
+    btMatrix3x3 rotation = trs.getBasis();
+
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+      Node& n = m_nodes[i];
+      n.m_x = rotation * (n.m_x - CoM) + CoM + translation;
+      n.m_q = rotation * (n.m_q - CoM) + CoM + translation;
+      n.m_n = rotation * n.m_n;
+      vol = btDbvtVolume::FromCR(n.m_x, margin);
+
+      m_ndbvt.update(n.m_leaf, vol);
+    }
+    updateNormals();
+    updateBounds();
+    updateConstants();
+  }
+
   // update modes
   updateModesByRotation(trs.getBasis());
 
@@ -419,7 +446,7 @@ void btReducedSoftBody::transform(const btTransform& trs)
   m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
   
   // update rigid frame (No need to update the rotation. Nodes have already been updated.)
-  m_rigidTransformWorld.setOrigin(trs.getOrigin());
+  m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin());
   m_interpolationWorldTransform = m_rigidTransformWorld;
   m_initialCoM = m_rigidTransformWorld.getOrigin();
 
@@ -428,6 +455,9 @@ void btReducedSoftBody::transform(const btTransform& trs)
 
 void btReducedSoftBody::scale(const btVector3& scl)
 {
+  // Scaling the mesh after transform is applied is not allowed
+  btAssert(!m_transform_lock);
+
   // scale the mesh
   {
     const btScalar margin = getCollisionShape()->getMargin();
@@ -464,6 +494,9 @@ void btReducedSoftBody::scale(const btVector3& scl)
 
 void btReducedSoftBody::setTotalMass(btScalar mass, bool fromfaces)
 {
+  // Changing the total mass after transform is applied is not allowed
+  btAssert(!m_transform_lock);
+
   btScalar scale_ratio = mass / m_mass;
 
   // update nodal mass

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h

@@ -20,6 +20,9 @@ class btReducedSoftBody : public btSoftBody
   typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 
  private:
+  // Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass.
+  bool m_transform_lock;
+
   // scaling factors
   btScalar m_rhoScale;         // mass density scale
   btScalar m_ksScale;          // stiffness scale
@@ -133,14 +136,6 @@ class btReducedSoftBody : public btSoftBody
   // need to use scale before using transform, because the scale is performed in the local frame 
   // (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info)
   virtual void scale(const btVector3& scl);
-  virtual void translate(const btVector3& trs)
-  {
-    btAssert(false); // use transform().
-  }
-  virtual void rotate(const btQuaternion& rot)
-  {
-    btAssert(false); // use transform().
-  }
 
  private:
   void updateRestNodalPositions();

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp

@@ -7,7 +7,8 @@
 btReducedSoftBody* btReducedSoftBodyHelpers::createReducedBeam(btSoftBodyWorldInfo& worldInfo, const int num_modes)
 {
 	std::string filepath("../../../data/reduced_beam/");
-	std::string filename = filepath + "beam_mesh.vtk";
+	// std::string filename = filepath + "beam_mesh.vtk";
+	std::string filename = filepath + "beam_mesh_origin.vtk";
 	btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(worldInfo, filename.c_str());
 	
 	rsb->setReducedModes(num_modes, rsb->m_nodes.size());