Merge remote-tracking branch 'bp/master'

This commit is contained in:
Erwin Coumans
2017-09-24 09:49:53 -07:00
7 changed files with 243 additions and 64 deletions

View File

@@ -69,7 +69,7 @@ idScalar maxAbsMat3x(const mat3x &m) {
void mul(const mat33 &a, const mat3x &b, mat3x *result) {
if (b.cols() != result->cols()) {
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}

View File

@@ -1013,7 +1013,7 @@ int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index,
int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans);
mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
return 0;
}