Checking constraint jacobian and normal directions.

master
Martin Felis 2020-11-08 20:51:45 +01:00
parent 3fac49ed9e
commit 5037eda185
4 changed files with 220 additions and 19 deletions

View File

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

View File

@ -153,20 +153,29 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
ground_shape.mType = SimShape::Plane; ground_shape.mType = SimShape::Plane;
ground_shape.pos = Vector3d::Zero(); ground_shape.pos = Vector3d::Zero();
ground_shape.orientation = Quaternion(0., 0., 0., 1.); 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; ground_body.mIsStatic = true;
double sphere_a_mass = 1.5; double sphere_a_mass = 1.5;
double sphere_b_mass = 1.5; double sphere_b_mass = 1.5;
SimBody sphere_a_body = CreateSphereBody( 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( 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; CollisionInfo cinfo;
SECTION ("SphereOnGroundButColliding") { SECTION("SphereOnGroundButColliding") {
sphere_a_body.q[1] = 0.5; sphere_a_body.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23; sphere_a_body.qdot[1] = -1.23;
@ -176,24 +185,58 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
REQUIRE(collisions.size() == 1); REQUIRE(collisions.size() == 1);
cinfo = collisions[0]; 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(cresult == true);
REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12); REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo); PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
CalcConstraintImpulse(&ground_body,&sphere_a_body, cinfo, 0); 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.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); ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12); 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<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; 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.q[1] = 0.5;
sphere_a_body.qdot[1] = -1.23; sphere_a_body.qdot[1] = -1.23;
@ -212,8 +255,8 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
// REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 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); // 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 impulse: " << sphere_a_body.qdot.transpose() << endl;
cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl; cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;

0
utils/__init__.py Normal file
View File

View File

@ -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 ()