Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Differences of generalized coordinates #389

Merged
merged 2 commits into from
May 26, 2015
Merged

Differences of generalized coordinates #389

merged 2 commits into from
May 26, 2015

Conversation

jslee02
Copy link
Member

@jslee02 jslee02 commented May 23, 2015

This pull request provides useful API for computing generalized velocity or acceleration using finite difference method. Previously, the finite differences for BallJoint and FreeJoint required special treatment that might not be straightforward.

Example code (when the root Joint is FreeJoint or BallJoint):

// time step and recorded generalized positions
double h = mWorld->getTimeStep();
Eigen::VectorXd q0 = mWorld->getRecording()->getConfig(49, 0);
Eigen::VectorXd q1 = mWorld->getRecording()->getConfig(50, 0);
Eigen::VectorXd q2 = mWorld->getRecording()->getConfig(51, 0);

// Old API
Eigen::VectorXd q_old = skeleton->getPositions();

skeleton->setPositions(q0);
Eigen::MatrixXd Jw0 = skeleton->getJoint(0)->getLocalJacobian().topLeftCorner<3,3>();
Eigen::VectorXd dq0 = (q1 - q0) / h; 
Eigen::Vector3d dq0_root = Jw0.inverse() * math::logMap(dart::math::expMapRot(-q0.head<3>()) * math::expMapRot(q1.head<3>())) / h;
dq0.head(3) = dq0_root;

skeleton->setPositions(q1);
Eigen::MatrixXd Jw1 = skeleton->getJoint(0)->getLocalJacobian().topLeftCorner<3,3>();
Eigen::VectorXd dq1 = (q2 - q1) / h; 
Eigen::Vector3d qd1_root = Jw1.inverse() * math::logMap(dart::math::expMapRot(-q.head<3>()) * math::expMapRot(postq.head(3))) / h;
dq1.head(3) = rootRotation;

Eigen::VectorXd ddq = (dq1 - dq0) / h;

skeleton->setPositions(q_old);

// New API
Eigen::VectorXd dq0 = skeleton->getPositionDifferences(q0, q1) / h;
Eigen::VectorXd dq1 = skeleton->getPositionDifferences(q1, q2) / h;
Eigen::VectorXd ddq = skeleton->getVelocityDifferences(dq0, dq1) / h;

Also, getting differences from a Skeleton with new generalized coordinates does not affect on the Skeleton's generalized coordinates so that we don't need to backup.

Resolves #290.

@jslee02 jslee02 added this to the Release DART 5.0 milestone May 23, 2015
@karenliu
Copy link
Member

+1

karenliu added a commit that referenced this pull request May 26, 2015
@karenliu
Merge pull request #389 from dartsim/finite_differences
d1ec56c 
Differences of generalized coordinates
@karenliu karenliu merged commit d1ec56c into master May 26, 2015
@jslee02 jslee02 mentioned this pull request May 26, 2015
Merged
@jslee02 jslee02 deleted the finite_differences branch May 31, 2015 00:08
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
DART 5.0.0
Development

Successfully merging this pull request may close these issues.

Provide API for finite difference
2 participants
@jslee02 @karenliu