Checking constraint jacobian and normal directions.

src/rbdlsim.cc

@@ -292,6 +292,7 @@ void CalcImpulseVariables(
     VectorNd* jac,
     double* G_MInv_GT) {
   if (body == nullptr || body->mIsStatic) {
+    jac->setZero();
     *G_MInv_GT = 0.;
     return;
   }
@@ -326,7 +327,7 @@ void PrepareConstraintImpulse(
       body_a,
       cinfo.mBodyAIndex,
       cinfo.posA,
-      cinfo.dir,
+      -cinfo.dir,
       &cinfo.MInvA,
       &cinfo.jacA,
       &cinfo.GMInvGTA);
@@ -334,7 +335,7 @@ void PrepareConstraintImpulse(
       body_b,
       cinfo.mBodyBIndex,
       cinfo.posB,
-      -cinfo.dir,
+      cinfo.dir,
       &cinfo.MInvB,
       &cinfo.jacB,
       &cinfo.GMInvGTB);
@@ -355,14 +356,14 @@ void CalcConstraintImpulse(
     rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacA * body_a->qdot;
   }
   if (body_b && !body_b->mIsStatic) {
-    rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
+    rhs -= (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot);
   }
 
   double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
 
   if (body_a && !body_a->mIsStatic) {
     double old_impulse = cinfo.accumImpulseA;
-    cinfo.deltaImpulseA = -rhs / denom;
+    cinfo.deltaImpulseA = rhs / denom;
     cinfo.accumImpulseA += cinfo.deltaImpulseA;
     cinfo.accumImpulseA = std::max(0., cinfo.accumImpulseA);
     cinfo.deltaImpulseA = cinfo.accumImpulseA - old_impulse;
@@ -370,7 +371,7 @@ void CalcConstraintImpulse(
 
   if (body_b && !body_b->mIsStatic) {
     double old_impulse = cinfo.accumImpulseB;
-    cinfo.deltaImpulseB = -rhs / denom;
+    cinfo.deltaImpulseB = rhs / denom;
     cinfo.accumImpulseB += cinfo.deltaImpulseB;
     cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB);
     cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse;
@@ -387,7 +388,7 @@ void ApplyConstraintImpulse(
   }
 
   if (body_b && !body_b->mIsStatic) {
-    body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
+    body_b->qdot -= cinfo.MInvB * cinfo.jacB.transpose()
                     * (cinfo.deltaImpulseB);
   }
 }

tests/CollisionTests.cc

@@ -153,16 +153,25 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
   ground_shape.mType = SimShape::Plane;
   ground_shape.pos = Vector3d::Zero();
   ground_shape.orientation = Quaternion(0., 0., 0., 1.);
-  ground_body.mCollisionShapes.push_back(SimBody::BodyCollisionInfo(-1, ground_shape));
+  ground_body.mCollisionShapes.push_back(
+      SimBody::BodyCollisionInfo(-1, ground_shape));
   ground_body.mIsStatic = true;
 
   double sphere_a_mass = 1.5;
   double sphere_b_mass = 1.5;
 
   SimBody sphere_a_body = CreateSphereBody(
-      sphere_a_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
+      sphere_a_mass,
+      1.0,
+      0.,
+      Vector3d(0., 0.5, 0.),
+      Vector3d(0., -1., 0.));
   SimBody sphere_b_body = CreateSphereBody(
-      sphere_b_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
+      sphere_b_mass,
+      1.0,
+      0.,
+      Vector3d(0., 0.5, 0.),
+      Vector3d(0., -1., 0.));
 
   CollisionInfo cinfo;
 
@@ -176,24 +185,58 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE(collisions.size() == 1);
     cinfo = collisions[0];
 
-    bool cresult = CheckPenetration(ground_shape,
+    bool cresult = CheckPenetration(
-        sphere_a_body.mCollisionShapes[0].second, cinfo);
+        ground_shape,
+        sphere_a_body.mCollisionShapes[0].second,
+        cinfo);
     REQUIRE(cresult == true);
     REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
 
     PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
     CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
-    double reference_impulseB = sphere_a_mass * sphere_a_body.qdot[1];
+    double reference_impulseB = -sphere_a_mass * sphere_a_body.qdot[1];
     REQUIRE(cinfo.accumImpulseA < 1.0e-12);
-    REQUIRE(cinfo.accumImpulseB + reference_impulseB < 1.0e-12);
+    REQUIRE(cinfo.accumImpulseB - reference_impulseB < 1.0e-12);
 
     ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
     REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
   }
 
+  SECTION("SphereOnGroundButCollidingReverseBodyOrder") {
+    sphere_a_body.q[1] = 0.5;
+    sphere_a_body.qdot[1] = -1.23;
+
+    sphere_a_body.updateCollisionShapes();
+    std::vector<CollisionInfo> collisions;
+    CalcCollisions(sphere_a_body, ground_body, collisions);
+    REQUIRE(collisions.size() == 1);
+    cinfo = collisions[0];
+
+    bool cresult = CheckPenetration(
+        sphere_a_body.mCollisionShapes[0].second,
+        ground_shape,
+        cinfo);
+    REQUIRE(cresult == true);
+    REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);
+
+    PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
+    CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
+    double reference_impulseA = -sphere_a_mass * sphere_a_body.qdot[1];
+    REQUIRE(cinfo.accumImpulseA - reference_impulseA < 1.0e-12);
+    REQUIRE(cinfo.accumImpulseB < 1.0e-12);
+
+    ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
+    REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
+  }
+
   SECTION("SphereVsSphereCollision") {
     double sphere_b_mass = 1.5;
-    SimBody sphere_b_body = CreateSphereBody(sphere_b_mass, 1.0, 0., Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
+    SimBody sphere_b_body = CreateSphereBody(
+        sphere_b_mass,
+        1.0,
+        0.,
+        Vector3d(0., -0.5, 0.),
+        Vector3d(0., 1., 0.));
 
     sphere_a_body.q[1] = 0.5;
     sphere_a_body.qdot[1] = -1.23;

utils/simple_math_gdb_pretty_print.py

@@ -0,0 +1,157 @@
+# -*- coding: utf-8 -*-
+# This file is part of SimpleMath, a lightweight C++ template library
+# for linear algebra.
+#
+# Copyright (C) 2009 Benjamin Schindler <bschindler@inf.ethz.ch>
+#
+# This Source Code Form is subject to the terms of the Mozilla Public
+# License, v. 2.0. If a copy of the MPL was not distributed with this
+# file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+# Pretty printers for SimpleMath::Matrix
+# This is still pretty basic as the python extension to gdb is still pretty basic.
+# It cannot handle complex simplemath types and it doesn't support many of the other simplemath types
+# This code supports fixed size as well as dynamic size matrices
+
+# To use it:
+#
+# * Create a directory and put the file as well as an empty __init__.py in
+#   that directory.
+# * Create a ~/.gdbinit file, that contains the following:
+#      python
+#      import sys
+#      sys.path.insert(0, '/path/to/simplemath/printer/directory')
+#      from printers import register_simplemath_printers
+#      register_simplemath_printers (None)
+#      end
+
+import gdb
+import re
+import itertools
+from bisect import bisect_left
+
+# Basic row/column iteration code for use with Sparse and Dense matrices
+class _MatrixEntryIterator(object):
+
+	def __init__ (self, rows, cols):
+		self.rows = rows
+		self.cols = cols
+		self.currentRow = 0
+		self.currentCol = 0
+
+	def __iter__ (self):
+		return self
+
+	def next(self):
+		return self.__next__()  # Python 2.x compatibility
+
+	def __next__(self):
+		row = self.currentRow
+		col = self.currentCol
+		if self.currentRow >= self.rows:
+			raise StopIteration
+
+		self.currentCol = self.currentCol + 1
+		if self.currentCol >= self.cols:
+			self.currentCol = 0
+			self.currentRow = self.currentRow + 1
+
+		return (row, col)
+
+class SimpleMathMatrixPrinter:
+	"Print SimpleMath Matrix or Array of some kind"
+
+	def __init__(self, variety, val):
+		"Extract all the necessary information"
+
+		# Save the variety (presumably "Matrix" or "Array") for later usage
+		self.variety = variety
+
+		# The gdb extension does not support value template arguments - need to extract them by hand
+		type = val.type
+		if type.code == gdb.TYPE_CODE_REF:
+			type = type.target()
+		self.type = type.unqualified().strip_typedefs()
+		tag = self.type.tag
+		regex = re.compile('\<.*\>')
+		m = regex.findall(tag)[0][1:-1]
+		template_params = m.split(',')
+		template_params = [x.replace(" ", "") for x in template_params]
+
+		self.rows = 3 
+		self.cols = 1
+		self.innerType = self.type.template_argument(0)
+		print ("type: ", str(self.type))
+		self.data = val['mStorage']['mData']
+#		self.rows = val['mStorage']['mRows']
+#		self.cols = val['mStorage']['mCols']
+#
+#		self.innerType = self.type.template_argument(0)
+#
+#		self.val = val
+#
+#		# Fixed size matrices have a struct as their storage, so we need to walk through this
+#		self.data = self.val['mStorage']['mData']
+#		if self.data.type.code == gdb.TYPE_CODE_STRUCT:
+#			self.data = self.data['array']
+#			self.data = self.data.cast(self.innerType.pointer())
+
+	class _iterator(_MatrixEntryIterator):
+		def __init__ (self, rows, cols, dataPtr):
+			super(SimpleMathMatrixPrinter._iterator, self).__init__(rows, cols)
+
+			self.dataPtr = dataPtr
+
+		def __next__(self):
+
+			row, col = super(SimpleMathMatrixPrinter._iterator, self).__next__()
+
+			item = self.dataPtr.dereference()
+			self.dataPtr = self.dataPtr + 1
+			if (self.cols == 1): #if it's a column vector
+				return ('[%d]' % (row,), item)
+			elif (self.rows == 1): #if it's a row vector
+				return ('[%d]' % (col,), item)
+			return ('[%d,%d]' % (row, col), item)
+
+	def children(self):
+
+		return self._iterator(self.rows, self.cols, self.data)
+
+	def to_string(self):
+		return "SimpleMath::%s<%s,%d,%d,%s> (data ptr: %s)" % (self.variety, self.innerType, self.rows, self.cols, self.data)
+
+def build_simplemath_dictionary ():
+	pretty_printers_dict[re.compile('^SimpleMath::MatrixBase<.*>$')] = lambda val: SimpleMathMatrixPrinter("Matrix", val)
+	print(str(pretty_printers_dict))
+
+def register_simplemath_printers(obj):
+	"Register simplemath pretty-printers with objfile Obj"
+
+	if obj == None:
+		obj = gdb
+	obj.pretty_printers.append(lookup_function)
+
+def lookup_function(val):
+	"Look-up and return a pretty-printer that can print va."
+
+	type = val.type
+
+	if type.code == gdb.TYPE_CODE_REF:
+		type = type.target()
+	type = type.unqualified().strip_typedefs()
+
+	typename = type.tag
+	if typename == None:
+		return None
+
+	for function in pretty_printers_dict:
+		if function.search(typename):
+			return pretty_printers_dict[function](val)
+
+	return None
+
+pretty_printers_dict = {}
+
+build_simplemath_dictionary ()
+