diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index e6bb48b..558fdf1 100644 --- a/src/rbdlsim.cc +++ b/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); } } diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index 2e83b67..d9ed537 100644 --- a/tests/CollisionTests.cc +++ b/tests/CollisionTests.cc @@ -153,20 +153,29 @@ 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; - SECTION ("SphereOnGroundButColliding") { + SECTION("SphereOnGroundButColliding") { sphere_a_body.q[1] = 0.5; sphere_a_body.qdot[1] = -1.23; @@ -176,24 +185,58 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { REQUIRE(collisions.size() == 1); cinfo = collisions[0]; - bool cresult = CheckPenetration(ground_shape, - sphere_a_body.mCollisionShapes[0].second, cinfo); + bool cresult = CheckPenetration( + 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]; + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + 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 ("SphereVsSphereCollision") { + SECTION("SphereOnGroundButCollidingReverseBodyOrder") { + sphere_a_body.q[1] = 0.5; + sphere_a_body.qdot[1] = -1.23; + + sphere_a_body.updateCollisionShapes(); + std::vector 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; @@ -212,8 +255,8 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); -// REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12); -// REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12); + // REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12); + // REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12); cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl; cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl; diff --git a/utils/__init__.py b/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/utils/simple_math_gdb_pretty_print.py b/utils/simple_math_gdb_pretty_print.py new file mode 100644 index 0000000..0a4a25f --- /dev/null +++ b/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 +# +# 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 () +