Skip to content

Commit

Permalink
Fix FreeJoint velocity and position integration for articulated bodies (
Browse files Browse the repository at this point in the history 
#28)

#10 attempted to fix an integration issue when the COM offset from the link origin. However, this seems to work only for single body systems. With an articulated body, the fix in #10 actually caused errors that lead to fictitious forced being applied. This in essence reverts #10 while keeping the necessary changes for updateConstrainedTerms and adding more tests.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
  • Loading branch information
@azeey
azeey committed Jun 9, 2021
1 parent 5bfc027 commit 96a4a55
Show file tree
Hide file tree
Showing 6 changed files with 411 additions and 63 deletions.
87 changes: 32 additions & 55 deletions dart/dynamics/FreeJoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -589,77 +589,54 @@ bool FreeJoint::isCyclic(std::size_t _index) const
//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Vector6d& vel = getVelocitiesStatic();
const Eigen::Vector6d& accel = getAccelerationsStatic();
auto* bn = getChildBodyNode();
Eigen::Isometry3d Tcom;
// Transform from joint to center of mass of child body
Tcom = Eigen::Translation3d(
Joint::mAspectProperties.mT_ChildBodyToJoint.inverse()
* bn->getLocalCOM());
auto velAtCom = math::AdT(Tcom.inverse(), vel);
auto accelAtCom = math::AdT(Tcom.inverse(), accel);

const Eigen::Isometry3d Qcom = getQ() * Tcom;
const Eigen::Isometry3d Qdiff = convertToTransform(velAtCom * _dt);

const Eigen::Vector6d newVelAtCom = math::AdR(Qdiff.inverse(), velAtCom);
const Eigen::Vector6d newAccelAtCom = math::AdR(Qdiff.inverse(), accelAtCom);

const Eigen::Isometry3d QnextAtCom = Qcom * Qdiff;
const Eigen::Isometry3d Qnext = QnextAtCom * Tcom.inverse();

setVelocitiesStatic(math::AdT(Tcom, newVelAtCom));
setAccelerationsStatic(math::AdT(Tcom, newAccelAtCom));
const Eigen::Isometry3d Qdiff
= convertToTransform(getVelocitiesStatic() * _dt);
const Eigen::Isometry3d Qnext = getQ() * Qdiff;

setVelocitiesStatic(math::AdR(Qdiff.inverse(), getVelocitiesStatic()));
setAccelerationsStatic(math::AdR(Qdiff.inverse(), getAccelerationsStatic()));
setPositionsStatic(convertToPositions(Qnext));
}

//==============================================================================
void FreeJoint::integrateVelocities(double _dt)
{
const Eigen::Vector6d& vel = getVelocitiesStatic();
// Acceleration with additional term to take into account changing linear
// velocity in the inertial frame.
Eigen::Vector6d accelWithInertialTerm = getAccelerationsStatic();

// Not sure if we need a different inertia than the one from the child body
// node.
const Eigen::Matrix6d& mI = getChildBodyNode()->getSpatialInertia();
const Eigen::Matrix6d& artInvProjI = getInvProjArtInertia();
// Remove Coriolis term because the velocity will be updated when integrating
// position and that will account for the rotation of the frame.
accelWithInertialTerm.tail<3>()
-= (artInvProjI * math::dad(vel, mI * vel)).tail<3>();

// Integrating the acceleration gives us the new velocity of child body frame.
// But if there is any linear acceleration, the frame will be displaced. If we
// apply euler integration direcly on the spatial acceleration, it will
// produce the velocity of a point that is instantaneously coincident with the
// previous location of the child body frame. However, we want to compute the
// spatial velocity at the current location of the child body frame. To
// accomplish this, we first convert the linear portion of the spatial
// acceleration into classical linear acceleration and apply the integration.
Eigen::Vector6d accel = getAccelerationsStatic();
const Eigen::Vector6d &velBefore = getVelocitiesStatic();
accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>());
setVelocitiesStatic(math::integrateVelocity<math::SE3Space>(
getVelocitiesStatic(), accelWithInertialTerm, _dt));
// vel now points to the updated velocity
accelWithInertialTerm.tail<3>()
+= (artInvProjI * math::dad(vel, mI * vel)).tail<3>();
setAccelerationsStatic(accelWithInertialTerm);
getVelocitiesStatic(), accel, _dt));

// Since the velocity has been updated, we use the new velocity to recompute
// the spatial acceleration. This is needed to ensure that functions like
// BodyNode::getLinearAcceleration work properly.
const Eigen::Vector6d& velAfter = getVelocitiesStatic();
accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>());
setAccelerationsStatic(accel);
}

//==============================================================================
void FreeJoint::updateConstrainedTerms(double timeStep)
{
const double invTimeStep = 1.0 / timeStep;

const Eigen::Vector6d& vel = getVelocitiesStatic();
Eigen::Vector6d accelWithInertialTerm = getAccelerationsStatic();
const Eigen::Matrix6d& mI = getChildBodyNode()->getSpatialInertia();
const Eigen::Matrix6d& artInvI = getInvProjArtInertia();
// Remove Coriolis term because the velocity will be updated when integrating
// position and that will account for the rotation of the frame.
accelWithInertialTerm.tail<3>()
-= (artInvI * math::dad(vel, mI * vel)).tail<3>();
const Eigen::Vector6d &velBefore = getVelocitiesStatic();
Eigen::Vector6d accel = getAccelerationsStatic();
accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>());

setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges);

// vel now points to the updated velocity
accelWithInertialTerm.tail<3>()
+= (artInvI * math::dad(vel, mI * vel)).tail<3>();
setAccelerationsStatic(
accelWithInertialTerm + mVelocityChanges * invTimeStep);

const Eigen::Vector6d &velAfter = getVelocitiesStatic();
accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>());
setAccelerationsStatic(accel + mVelocityChanges * invTimeStep);
this->mAspectState.mForces.noalias() += mImpulses * invTimeStep;
// Note: As long as this is only called from BodyNode::updateConstrainedTerms
}
Expand Down
62 changes: 62 additions & 0 deletions data/sdf/test/issue1193_revolute_test.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
<sdf version="1.5">
<world name='default'>
<gravity>0 0 0</gravity>
<physics type="dart">
<gravity>0.0 0.0 0</gravity>
<real_time_update_rate>1000.000000</real_time_update_rate>
<max_step_size>0.001000</max_step_size>
</physics>
<model name="M">
<pose>1 1 0 0 0 0</pose>
<link name="link1">
<pose>0 0 2 0 0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>1.0</ixx>
<iyy>1.0</iyy>
<izz>1.0</izz>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyz>0.0</iyz>
</inertia>
</inertial>
<visual name="v1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
<visual name='v2'>
<pose>0 0 -2 0 0 0</pose>
<geometry><sphere><radius>0.1</radius></sphere></geometry>
</visual>
</link>
<joint name="revJoint" type="revolute">
<parent>link1</parent>
<child>link2</child>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<link name="link2">
<pose>0 0 -2 0 0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>1.0</ixx>
<iyy>1.0</iyy>
<izz>1.0</izz>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyz>0.0</iyz>
</inertia>
</inertial>
<visual name='v1'>
<geometry><box><size>1 1 1</size></box></geometry>
</visual>
</link>
</model>
</world>
</sdf>
62 changes: 62 additions & 0 deletions data/sdf/test/issue1193_revolute_with_offset_test.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
<sdf version="1.5">
<world name='default'>
<gravity>0 0 0</gravity>
<physics type="dart">
<gravity>0.0 0.0 0</gravity>
<real_time_update_rate>1000.000000</real_time_update_rate>
<max_step_size>0.001000</max_step_size>
</physics>
<model name="M">
<link name="link1">
<pose>0 0 2 0 0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>1.0</ixx>
<iyy>1.0</iyy>
<izz>1.0</izz>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyz>0.0</iyz>
</inertia>
</inertial>
<visual name="v1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
<visual name='v2'>
<pose>0 0 -2 0 0 0</pose>
<geometry><sphere><radius>0.1</radius></sphere></geometry>
</visual>
</link>
<joint name="revJoint" type="revolute">
<pose>0 0 2 0 0 0</pose>
<parent>link1</parent>
<child>link2</child>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<link name="link2">
<pose>0 0 -2 0 0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>1.0</ixx>
<iyy>1.0</iyy>
<izz>1.0</izz>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyz>0.0</iyz>
</inertia>
</inertial>
<visual name='v1'>
<geometry><box><size>1 1 1</size></box></geometry>
</visual>
</link>
</model>
</world>
</sdf>
20 changes: 12 additions & 8 deletions unittests/comprehensive/test_Dynamics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2531,7 +2531,9 @@ TEST_F(DynamicsTest, OffsetCOM)
box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass()));
inertia.setLocalCOM({0, 500, 0});
box->setInertia(inertia);
box->addExtTorque({0, 0, 100}, false);
Eigen::Isometry3d boxInitialPose = box->getWorldTransform();
Eigen::Vector3d torque(0, 0, 100);
box->addExtTorque(torque, false);

{
Vector3d comVel = box->getCOMLinearVelocity();
Expand All @@ -2548,28 +2550,30 @@ TEST_F(DynamicsTest, OffsetCOM)
// Velocity at COM should be zero since the object is just rotating.
{
Vector3d comVel = box->getCOMLinearVelocity();
EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel))
EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4))
<< "comVel: " << comVel.transpose();
}
{
Vector3d angVel = box->getAngularVelocity();
EXPECT_TRUE(equals(Vector3d(0, 0, 0.01), angVel))
// We can compute expectedAngAccel like so because the initial angular
// velocity is zero.
Vector3d expectedAngAccel = inertia.getMoment().inverse() * torque;
Vector3d expectedAngVel = expectedAngAccel * dt;
EXPECT_TRUE(equals(expectedAngVel, angVel))
<< "angVel: " << angVel.transpose();
Vector3d linVel = box->getLinearVelocity();
Vector3d expLinVel
= angVel.cross(box->getWorldTransform().linear() * -box->getLocalCOM());
= angVel.cross(boxInitialPose.linear() * -box->getLocalCOM());
EXPECT_TRUE(equals(expLinVel, linVel))
<< "Expected: " << expLinVel.transpose()
<< "\nActual: " << linVel.transpose();

Vector3d angAccel = box->getAngularAcceleration();
EXPECT_TRUE(equals(Vector3d(0, 0, 10), angAccel))
EXPECT_TRUE(equals(expectedAngAccel, angAccel))
<< "angAccel: " << angAccel.transpose();
Vector3d linAccel = box->getLinearAcceleration();
Vector3d expLinAccel
= angVel.cross(linVel)
+ angAccel.cross(
box->getWorldTransform().linear() * -box->getLocalCOM());
= expectedAngAccel.cross(boxInitialPose.linear() * -box->getLocalCOM());
EXPECT_TRUE(equals(expLinAccel, linAccel))
<< "Expected: " << expLinAccel.transpose()
<< "\nActual: " << linAccel.transpose();
Expand Down
1 change: 1 addition & 0 deletions unittests/regression/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,3 +35,4 @@ if(TARGET dart-collision-bullet AND TARGET dart-collision-ode)
endif()

dart_add_test("regression" test_Issue1193)
target_link_libraries(test_Issue1193 dart-utils)
Loading

0 comments on commit 96a4a55

Please sign in to comment.