diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 5ae2ad5a41f47..9232ca1b11f55 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/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); } //============================================================================== diff --git a/dart/dynamics/FreeJoint.hpp b/dart/dynamics/FreeJoint.hpp index fcaf25ca02777..19aec1a25a1a2 100644 --- a/dart/dynamics/FreeJoint.hpp +++ b/dart/dynamics/FreeJoint.hpp @@ -202,7 +202,8 @@ class FreeJoint : public GenericJoint /// 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 /// 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); diff --git a/tests/integration/dynamics/test_Joints.cpp b/tests/integration/dynamics/test_Joints.cpp index 980f17cc6489a..a62419549eff8 100644 --- a/tests/integration/dynamics/test_Joints.cpp +++ b/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* 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(numSteps); + const Eigen::Vector3d actualTranslation + = body->getWorldTransform().translation() - initialTranslation; + EXPECT_VECTOR_NEAR(expectedTranslation, actualTranslation, 1e-6); +} + //============================================================================== template void JOINTS::kinematicsTest(const typename JointType::Properties& _properties)