RigidObject velocity control #486

aclegg3 · 2020-02-11T00:09:23Z

Motivation and Context

This change introduces the VelocityControl class for setting constant linear|angular control velocities for RigidObjects. The PhysicsManager::stepPhysics() and derived variants consume this class to either set initial simulation velocities (MotionType::DYNAMIC), or integrate velocities to set new object states (MotionType::KINEMATIC).

The VelocityControl::linVelIsLocal variable allows the user to define local linear velocities in order to implement commands such as "forward" (e.g. [0.0,0.0,-1.0]) and "back" easily. In this case the linear velocity vector is rotated by the object's orientation transform before application.

Kinematic integration is done with VelocityControl::integrateTransform() which implements explicit Euler integration native to Habitat (i.e. does not require Bullet). This can be overrode in the future to introduce new integrators if necessary. Note: this form of velocity control is not constrained by collision.

How Has This Been Tested

New C++ unit tests and viewer demo testing (not included in this change):

Example with MotionType::DYNAMIC:

Example with MotionType::KINEMATIC:

Example with random angular velocity (to demonstrate 3D flight):

Types of changes

Docs change / refactoring / dependency upgrade
Bug fix (non-breaking change which fixes an issue)
New feature (non-breaking change which adds functionality)
Breaking change (fix or feature that would cause existing functionality to change)

Checklist

My code follows the code style of this project.
My change requires a change to the documentation.
I have updated the documentation accordingly.
I have read the CONTRIBUTING document.
I have completed my CLA (see CONTRIBUTING)
I have added tests to cover my changes.
All new and existing tests passed.

…implemented through BulletRigidBody. Added test skeleton and viewer demo.

…ruct

…nd BulletPhysicsManager. Updated viewer demo.

…ruct

codecov · 2020-02-11T19:11:07Z

Codecov Report

Merging #486 into master will increase coverage by 0.63%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##           master     #486      +/-   ##
==========================================
+ Coverage   59.19%   59.83%   +0.63%     
==========================================
  Files         165      165              
  Lines        7340     7424      +84     
  Branches       84       84              
==========================================
+ Hits         4345     4442      +97     
+ Misses       2995     2982      -13

Flag	Coverage Δ
#CPP	`55.72% <84.70%> (+1.16%)`	⬆️
#JavaScript	`10.00% <ø> (ø)`
#Python	`79.72% <ø> (ø)`

Impacted Files	Coverage Δ
src/esp/physics/PhysicsManager.cpp	`50.61% <0.00%> (+1.88%)`	⬆️
src/esp/physics/bullet/BulletRigidObject.cpp	`73.97% <0.00%> (+3.76%)`	⬆️
src/esp/physics/RigidObject.cpp	`44.16% <0.00%> (+9.26%)`	⬆️
src/esp/physics/PhysicsManager.h	`36.36% <0.00%> (+4.54%)`	⬆️
src/esp/physics/bullet/BulletPhysicsManager.cpp	`63.43% <0.00%> (+6.28%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 05db83e...4de4e9a. Read the comment docs.

mosra

Haha, looking at the animations at first I thought you were hooking up some easing curves from the Easing namespace :)

src/esp/physics/RigidObject.cpp

aclegg3 · 2020-02-12T21:14:27Z

Haha, looking at the animations at first I thought you were hooking up some easing curves from the Easing namespace :)

Does seem that way, haha. Thanks for pointing those out, I'll have to take a look. We've been considering some curve utilities. 👍

src/esp/physics/PhysicsManager.cpp

src/esp/physics/RigidObject.h

bigbike · 2020-03-04T19:49:20Z

src/esp/physics/RigidObject.cpp

@@ -213,5 +213,35 @@ Magnum::Matrix3 RigidObject::getInertiaMatrix() {
  return inertia;
 }

+//////////////////
+// VelocityControl
+Magnum::Matrix4 VelocityControl::integrateTransform(


@mosra: have you read this function?
Any advanced matrix manipulation from Magnum library can kick in to improve it?

See my earlier comments.

bigbike · 2020-03-04T20:36:27Z

src/esp/physics/RigidObject.cpp

+    }
+    Magnum::Quaternion q = Magnum::Quaternion::rotation(
+        Magnum::Rad{(globalAngVel * dt).length()}, globalAngVel.normalized());
+    newRotationScaling = q.toMatrix() * newRotationScaling;


Just be careful.
It may cause significant numerical drift here (even with "double"). Not so sure if it would an issue in the future.

One option to consider in the future might be switching to TranslationRotationScalingTransformation3D that stores rotation as a quat and thus could allow for faster and more precise calculation without having to go through matrices. However extra care has to be taken as relative order of the T/R/S transformations is different than with MatrixTransformation3D.

Another option might be DualQuaternionTransformation but while it reduces the amount of ALU ops even further, it is more restrictive as there's no shear or scaling.

Storing and time stepping rotation using a unit quaternion, or exponential map is the typical way we do in rigid body simulation.

Applying DualQuaternion method to handle rigid-body dynamics is rare. It is initially introduced by Kavan to computer animation to avoid the artifacts shown in linear skinning.

rare

Yes, I know it got first widely used for skinning, but that's not relevant in this case. Dual quats are, at its core, an efficient way to store translation+rotation together, so why not use it like that. The only thing related to animation/skinning is the DLB operation, and that's not used here.

so why not use it like that.

I am talking about the the rigid body dynamics system in general. Because dual quaternion will make it much more complicated than it should be. A rigid body has only 6 DOFs but the dual quaternion method provides 8 DOFs + 2 contraints. Such redundancy makes the 1st and 2nd derivatives of generalized coordinates q w.r.t time t (dq/dt) difficult to derive.
So typically in a dynamics system, we decouple the state to rotation + translation, and apply exponential map (which has exact 3 DOFs, compared to quaternion, which has 4 DOFs + 1 constraint) to handle rotation change. In this case, you can still employ a unit quaternion to accumulate such orientation changes (and represent the orientation state at any frame).

Dual quaternion is good at handling the kinematics, which has nothing to do with the time derivatives.

In our simulator, whether we use dual quaternion depends on our future needs. If we would like to do some research of our own on "dynamics systems", not just "kinematics", we need to carefully choose the generalized coordinates, and maintain the internal states. In this case, I will not go with dual quaternion.

If we keep ourselves as customers to the physics libraries such as bullet, completely leaving the dynamics to it, or doing some very simple, explicit integration (not implicit integration), then sure we can use the dual quaternion.

Thanks for the good discussion on state representation. We should continue to keep an eye on our use cases moving forward. If we find that our current representation is lacking in speed or accuracy we can revisit the alternatives. 👍

bigbike · 2020-03-05T22:51:19Z

src/esp/physics/RigidObject.cpp

+    }
+    Magnum::Quaternion q = Magnum::Quaternion::rotation(
+        Magnum::Rad{(globalAngVel * dt).length()}, globalAngVel.normalized());
+    newRotationScaling = q.toMatrix() * newRotationScaling;


so why not use it like that.

I am talking about the the rigid body dynamics system in general. Because dual quaternion will make it much more complicated than it should be. A rigid body has only 6 DOFs but the dual quaternion method provides 8 DOFs + 2 contraints. Such redundancy makes the 1st and 2nd derivatives of generalized coordinates q w.r.t time t (dq/dt) difficult to derive.
So typically in a dynamics system, we decouple the state to rotation + translation, and apply exponential map (which has exact 3 DOFs, compared to quaternion, which has 4 DOFs + 1 constraint) to handle rotation change. In this case, you can still employ a unit quaternion to accumulate such orientation changes (and represent the orientation state at any frame).

Dual quaternion is good at handling the kinematics, which has nothing to do with the time derivatives.

In our simulator, whether we use dual quaternion depends on our future needs. If we would like to do some research of our own on "dynamics systems", not just "kinematics", we need to carefully choose the generalized coordinates, and maintain the internal states. In this case, I will not go with dual quaternion.

If we keep ourselves as customers to the physics libraries such as bullet, completely leaving the dynamics to it, or doing some very simple, explicit integration (not implicit integration), then sure we can use the dual quaternion.

* Add VelocityControl structure for setting desired constant velocity in world and local frame. Interface with PhysicsManager and BulletPhysicsManager. * Add kinematic integrator for velocity control without dynamics. * Added tests for velocity control

* Add fisheye camera model * Add ObservationTransformer for fisheye camera * Add test for CubeMap2Fisheye * Add comment about future refactor * Update tests Co-authored-by: Aaron Gokaslan <agokaslan@fb.com>

aclegg3 added 11 commits January 30, 2020 09:07

Add get/set linear/angular velocity for RigidBody and PhysicsManager …

6711416

…implemented through BulletRigidBody. Added test skeleton and viewer demo.

const getters

2e97f47

Update test and viewer demo

5ef0ec1

Updated docs for new functions

872c637

remove unnecessary CORRADE_UNUSED specifiers

3476de8

demo

4fd43c7

Merge remote-tracking branch 'origin/master' into velocity-control-st…

17ec713

…ruct

Merge remote-tracking branch 'origin/master' into velocity-control-st…

c35fce8

…ruct

VelocityControl kinematic integrator. Interface with PhysicsManager a…

5543d86

…nd BulletPhysicsManager. Updated viewer demo.

refine velocity control tests

a464627

remove config and viewer changes

d685d96

aclegg3 requested review from mathfac, bigbike, abhiskk, vedanuj and msbaines February 11, 2020 00:09

aclegg3 self-assigned this Feb 11, 2020

facebook-github-bot added the CLA Signed Do not delete this pull request or issue due to inactivity. label Feb 11, 2020

aclegg3 added 3 commits February 10, 2020 16:10

Merge remote-tracking branch 'origin/master' into velocity-control-st…

2a44e2f

…ruct

Fix implicit conversions causing clang-tidy issues

c071cb2

fix CI scope issue with isnan

e484741

mosra reviewed Feb 12, 2020

View reviewed changes

src/esp/physics/RigidObject.cpp Outdated Show resolved Hide resolved

Simplified angular velocity integration

3e3e7f4

Added local angular to VelocityControl

cc16e83

bigbike suggested changes Mar 4, 2020

View reviewed changes

bigbike reviewed Mar 4, 2020

View reviewed changes

aclegg3 and others added 2 commits March 4, 2020 12:58

Change VelocityControl from class to struct

f839338

precommit cleanup

4de4e9a

bigbike approved these changes Mar 5, 2020

View reviewed changes

aclegg3 merged commit e751091 into master Mar 6, 2020

aclegg3 deleted the velocity-control-struct branch March 6, 2020 15:44

aclegg3 mentioned this pull request Apr 3, 2020

Controller for Habitat Agent facebookresearch/habitat-lab#356

Closed

dhruvbatra mentioned this pull request May 2, 2020

Interactive RigidObject python tutorial page #611

Merged

11 tasks

Skylion007 mentioned this pull request Sep 25, 2020

Support for fisheye image in Magnum #794

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

RigidObject velocity control #486

RigidObject velocity control #486

aclegg3 commented Feb 11, 2020

codecov bot commented Feb 11, 2020 •

edited

Loading

mosra left a comment

aclegg3 commented Feb 12, 2020

bigbike Mar 4, 2020 •

edited

Loading

mosra Mar 4, 2020

bigbike Mar 4, 2020

mosra Mar 4, 2020

bigbike Mar 4, 2020

mosra Mar 5, 2020

bigbike Mar 5, 2020 •

edited

Loading

aclegg3 Mar 6, 2020

bigbike Mar 5, 2020 •

edited

Loading

RigidObject velocity control #486

RigidObject velocity control #486

Conversation

aclegg3 commented Feb 11, 2020

Motivation and Context

How Has This Been Tested

Types of changes

Checklist

codecov bot commented Feb 11, 2020 • edited Loading

Codecov Report

mosra left a comment

Choose a reason for hiding this comment

aclegg3 commented Feb 12, 2020

bigbike Mar 4, 2020 • edited Loading

Choose a reason for hiding this comment

mosra Mar 4, 2020

Choose a reason for hiding this comment

bigbike Mar 4, 2020

Choose a reason for hiding this comment

mosra Mar 4, 2020

Choose a reason for hiding this comment

bigbike Mar 4, 2020

Choose a reason for hiding this comment

mosra Mar 5, 2020

Choose a reason for hiding this comment

bigbike Mar 5, 2020 • edited Loading

Choose a reason for hiding this comment

aclegg3 Mar 6, 2020

Choose a reason for hiding this comment

bigbike Mar 5, 2020 • edited Loading

Choose a reason for hiding this comment

codecov bot commented Feb 11, 2020 •

edited

Loading

bigbike Mar 4, 2020 •

edited

Loading

bigbike Mar 5, 2020 •

edited

Loading

bigbike Mar 5, 2020 •

edited

Loading