Document FreeJoint velocity actuator workflow

dart/dynamics/FreeJoint.cpp

@@ -205,8 +205,10 @@ void FreeJoint::setTransform(
 void FreeJoint::setRelativeSpatialVelocity(
     const Eigen::Vector6d& newSpatialVelocity)
 {
-  setVelocitiesStatic(
-      getRelativeJacobianStatic().inverse() * newSpatialVelocity);
+  const Eigen::Vector6d jointVelocities
+      = getRelativeJacobianStatic().inverse() * newSpatialVelocity;
+  Eigen::VectorXd velocityVector = jointVelocities;
+  setVelocities(velocityVector);
 }
 
 //==============================================================================
@@ -347,8 +349,10 @@ void FreeJoint::setRelativeSpatialAcceleration(
   const Eigen::Matrix6d& J = getRelativeJacobianStatic();
   const Eigen::Matrix6d& dJ = getRelativeJacobianTimeDerivStatic();
 
-  setAccelerationsStatic(
-      J.inverse() * (newSpatialAcceleration - dJ * getVelocitiesStatic()));
+  const Eigen::Vector6d jointAccelerations
+      = J.inverse() * (newSpatialAcceleration - dJ * getVelocitiesStatic());
+  Eigen::VectorXd accelerationVector = jointAccelerations;
+  setAccelerations(accelerationVector);
 }
 
 //==============================================================================

dart/dynamics/FreeJoint.hpp

@@ -202,7 +202,8 @@ class FreeJoint : public GenericJoint<math::SE3Space>
   /// BodyNode.
   /// \param[in] newSpatialVelocity Desired spatial velocity of the child
   /// BodyNode. The reference frame of "newSpatialVelocity" is the child
-  /// BodyNode.
+  /// BodyNode. This updates generalized velocities via `setVelocities()` so
+  /// VELOCITY actuator commands stay synchronized.
   void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity);
 
   /// Set the spatial velocity of the child BodyNode relative to the parent
@@ -256,7 +257,8 @@ class FreeJoint : public GenericJoint<math::SE3Space>
   /// BodyNode.
   /// \param[in] newSpatialAcceleration Desired spatial acceleration of the
   /// child BodyNode. The reference frame of "newSpatialAcceleration" is the
-  /// child BodyNode.
+  /// child BodyNode. This updates generalized accelerations via
+  /// `setAccelerations()` so ACCELERATION actuator commands stay synchronized.
   void setRelativeSpatialAcceleration(
       const Eigen::Vector6d& newSpatialAcceleration);
 

tests/integration/dynamics/test_Joints.cpp

@@ -139,6 +139,51 @@ void JOINTS::randomizeRefFrames()
   }
 }
 
+//==============================================================================
+TEST_F(JOINTS, FREE_JOINT_SPATIAL_VELOCITY_WITH_VELOCITY_ACTUATOR)
+{
+  auto world = simulation::World::create();
+  const double timeStep = 0.01;
+  world->setTimeStep(timeStep);
+  world->setGravity(Eigen::Vector3d::Zero());
+
+  auto skeleton = Skeleton::create("free_joint_kinematic");
+  auto pair = skeleton->createJointAndBodyNodePair<FreeJoint>();
+  FreeJoint* joint = pair.first;
+  BodyNode* body = pair.second;
+
+  joint->setActuatorType(Joint::VELOCITY);
+  world->addSkeleton(skeleton);
+
+  Eigen::Vector6d desiredVel = Eigen::Vector6d::Zero();
+  desiredVel.tail<3>() << 0.3, -0.1, 0.2;
+
+  joint->setSpatialVelocity(desiredVel, Frame::World(), Frame::World());
+
+  const Eigen::Vector6d initialVel
+      = body->getSpatialVelocity(Frame::World(), Frame::World());
+  EXPECT_VECTOR_NEAR(desiredVel, initialVel, 1e-8);
+
+  const Eigen::Vector3d initialTranslation
+      = body->getWorldTransform().translation();
+
+  const std::size_t numSteps = 5;
+  for (std::size_t i = 0; i < numSteps; ++i) {
+    joint->setSpatialVelocity(desiredVel, Frame::World(), Frame::World());
+    world->step();
+  }
+
+  const Eigen::Vector6d actualVel
+      = body->getSpatialVelocity(Frame::World(), Frame::World());
+  EXPECT_VECTOR_NEAR(desiredVel, actualVel, 1e-8);
+
+  const Eigen::Vector3d expectedTranslation
+      = desiredVel.tail<3>() * timeStep * static_cast<double>(numSteps);
+  const Eigen::Vector3d actualTranslation
+      = body->getWorldTransform().translation() - initialTranslation;
+  EXPECT_VECTOR_NEAR(expectedTranslation, actualTranslation, 1e-6);
+}
+
 //==============================================================================
 template <typename JointType>
 void JOINTS::kinematicsTest(const typename JointType::Properties& _properties)