Skip to content

Commit 3c359c7

Browse files
committed
Fix revolute constraint angular Jacobian frame
1 parent 6746f65 commit 3c359c7

File tree

1 file changed

+4
-2
lines changed

1 file changed

+4
-2
lines changed

dart/constraint/RevoluteJointConstraint.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -193,12 +193,14 @@ void RevoluteJointConstraint::update()
193193

194194
mJacobian1.row(3).setZero();
195195
mJacobian1.row(4).setZero();
196-
mJacobian1.block<2, 3>(3, 0) = mPerpBasis;
196+
mJacobian1.block<2, 3>(3, 0)
197+
= mPerpBasis * mBodyNode1->getTransform().linear();
197198

198199
if (mBodyNode2) {
199200
mJacobian2.row(3).setZero();
200201
mJacobian2.row(4).setZero();
201-
mJacobian2.block<2, 3>(3, 0) = mPerpBasis;
202+
mJacobian2.block<2, 3>(3, 0)
203+
= mPerpBasis * mBodyNode2->getTransform().linear();
202204
}
203205

204206
if (mBodyNode2) {

0 commit comments

Comments
 (0)