martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Explicitly formulating bias term (missed some changes)

master
Martin Felis 2020-11-15 21:47:17 +01:00
parent 4a8d691334
commit 3377de6b81
3 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h vendored
View File

@ -1233,6 +1233,12 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
return *this; return *this;
} }
Matrix& operator+=(const ScalarType& scalar) {
assert (rows() == 1 && cols() == 1 && "Error: matrix dimensions do not match!");
this->operator()(0,0) += scalar;
return *this;
}
template <typename OtherDerived> template <typename OtherDerived>
Matrix& operator-=(const OtherDerived& other) { Matrix& operator-=(const OtherDerived& other) {
assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!"); assert (rows() == other.rows() && cols() == other.cols() && "Error: matrix dimensions do not match!");
@ -1244,6 +1250,12 @@ struct Matrix : public MatrixBase<Matrix<ScalarType, NumRows, NumCols>, ScalarTy
return *this; return *this;
} }
Matrix& operator-=(const ScalarType& scalar) {
assert (rows() == 1 && cols() == 1 && "Error: matrix dimensions do not match!");
this->operator()(0,0) -= scalar;
return *this;
}
inline ScalarType& operator()(const size_t& i, const size_t& j) { inline ScalarType& operator()(const size_t& i, const size_t& j) {
return mStorage.coeff(i, j); return mStorage.coeff(i, j);
} }

2
include/rbdlsim.h
View File

@ -49,6 +49,8 @@ struct CollisionInfo {
int mBodyBIndex; int mBodyBIndex;
Vector3d posA = Vector3d::Zero(); Vector3d posA = Vector3d::Zero();
Vector3d posB = Vector3d::Zero(); Vector3d posB = Vector3d::Zero();
double biasVelocityA = 0.;
double biasVelocityB = 0.;
double accumImpulse = 0.; double accumImpulse = 0.;
double deltaImpulse = 0.; double deltaImpulse = 0.;
Vector3d dir = Vector3d::Zero(); Vector3d dir = Vector3d::Zero();

2
tests/CollisionTests.cc
View File

@ -235,6 +235,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
SECTION("CheckBounce") { SECTION("CheckBounce") {
cinfo.effectiveRestitution = 1.0; cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
VectorNd old_vel = sphere_a_body.qdot; VectorNd old_vel = sphere_a_body.qdot;
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo); ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
@ -339,6 +340,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
SECTION("CheckBounce") { SECTION("CheckBounce") {
cinfo.effectiveRestitution = 1.0; cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
VectorNd old_vel_a = sphere_a_body.qdot; VectorNd old_vel_a = sphere_a_body.qdot;
VectorNd old_vel_b = sphere_b_body.qdot; VectorNd old_vel_b = sphere_b_body.qdot;
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);