Add support for animation tension

graphics/include/ignition/common/Animation.hh

@@ -117,6 +117,21 @@ namespace ignition
  public: PoseAnimation(const std::string &_name,
  const double _length, const bool _loop);
 
+ /// \brief Constructor
+ /// \param[in] _name String name of the animation. This should be unique.
+ /// \param[in] _length Length of the animation in seconds
+ /// \param[in] _loop True == loop the animation
+ /// \param[in] _tension The tension of the trajectory spline. The
+ /// default value of zero equates to a Catmull-Rom spline, which may
+ /// also cause the animation to overshoot keyframes. A value of one will
+ /// cause the animation to stick to the keyframes. This value should
+ /// be in the range 0..1.
+ /// \todo(nkoenig) Remove this in ign-common5, and use a single
+ /// consutrctory with a default _tension of 0.
+ public: PoseAnimation(const std::string &_name,
+ const double _length, const bool _loop,
+ double _tension);
+
  /// \brief Create a pose keyframe at the given time
  /// \param[in] _time Time at which to create the keyframe
  /// \return Pointer to the new keyframe
@@ -234,6 +249,18 @@ namespace ignition
  std::map<std::chrono::steady_clock::time_point, math::Pose3d>
  _waypoints);
 
+ /// \brief Load all waypoints in the trajectory
+ /// \param[in] _waypoints Map of waypoints, where the key is the absolute
+ /// time of the waypoint and the value is the pose.
+ /// \param[in] _tension The tension of the trajectory spline. The
+ /// default value of zero equates to a Catmull-Rom spline, which may
+ /// also cause the animation to overshoot keyframes. A value of one will
+ /// cause the animation to stick to the keyframes. This value should
+ /// be in the range 0..1.
+ public: void SetWaypoints(
+ std::map<std::chrono::steady_clock::time_point, math::Pose3d>
+ _waypoints, double _tension);
+
  /// \brief Private data pointer.
  IGN_UTILS_IMPL_PTR(dataPtr)
  };

graphics/src/Animation.cc

@@ -91,6 +91,9 @@ class PoseAnimation::Implementation
 
  /// \brief determines if the interpolation splines need building
  public: bool build{false};
+
+ /// \brief Spline tension parameter.
+ public: double tension{0.0};
 };
 
 /////////////////////////////////////////////////
@@ -290,6 +293,15 @@ PoseAnimation::PoseAnimation(const std::string &_name, const double _length,
 {
 }
 
+/////////////////////////////////////////////////
+PoseAnimation::PoseAnimation(const std::string &_name, const double _length,
+ const bool _loop, double _tension)
+: Animation(_name, _length, _loop),
+ dataPtr(ignition::utils::MakeImpl<Implementation>())
+{
+ this->dataPtr->tension = math::clamp(_tension, 0.0, 1.0);
+}
+
 /////////////////////////////////////////////////
 PoseKeyFrame *PoseAnimation::CreateKeyFrame(const double _time)
 {
@@ -309,6 +321,7 @@ void PoseAnimation::BuildInterpolationSplines()
  this->dataPtr->positionSpline->AutoCalculate(false);
  this->dataPtr->rotationSpline->AutoCalculate(false);
 
+ this->dataPtr->positionSpline->Tension(this->dataPtr->tension);
  this->dataPtr->positionSpline->Clear();
  this->dataPtr->rotationSpline->Clear();
 
@@ -511,6 +524,15 @@ common::PoseAnimation *TrajectoryInfo::Waypoints() const
 void TrajectoryInfo::SetWaypoints(
  std::map<std::chrono::steady_clock::time_point, math::Pose3d> _waypoints)
 {
+ this->SetWaypoints(_waypoints, 0.0);
+}
+
+/////////////////////////////////////////////////
+void TrajectoryInfo::SetWaypoints(
+ std::map<std::chrono::steady_clock::time_point, math::Pose3d> _waypoints,
+ double _tension)
+{
+ _tension = math::clamp(_tension, 0.0, 1.0);
  this->dataPtr->segDistance.clear();
 
  auto first = _waypoints.begin();
@@ -523,7 +545,7 @@ void TrajectoryInfo::SetWaypoints(
  animName << this->AnimIndex() << "_" << this->Id();
  std::shared_ptr<common::PoseAnimation> anim =
  std::make_shared<common::PoseAnimation>(animName.str(),
- std::chrono::duration<double>(this->Duration()).count(), false);
+ std::chrono::duration<double>(this->Duration()).count(), false, _tension);
 
  auto prevPose = first->second.Pos();
  for (auto pIter = _waypoints.begin(); pIter != _waypoints.end(); ++pIter)

graphics/src/Animation_TEST.cc

@@ -87,6 +87,30 @@ TEST_F(AnimationTest, PoseAnimation)
  math::Quaterniond(0.0302776, 0.0785971, 0.109824));
 }
 
+/////////////////////////////////////////////////
+TEST_F(AnimationTest, PoseAnimationTension)
+{
+ // Test using a tension value of 1.0, which should cause the pose
+ // animation to hit the key frames.
+
+ common::PoseAnimation animTension("test-tension", 10.0, false, 1.0);
+ common::PoseKeyFrame *keyTension = animTension.CreateKeyFrame(0.0);
+ keyTension->Translation(math::Vector3d(0, 0, 0));
+ keyTension->Rotation(math::Quaterniond(0, 0, 0));
+
+ animTension.AddTime(5.0);
+ keyTension->Translation(math::Vector3d(10, 20, 30));
+ keyTension->Rotation(math::Quaterniond(0.1, 0.2, 0.3));
+ animTension.Time(4.0);
+
+ common::PoseKeyFrame interpolatedKeyTension(-1.0);
+ animTension.InterpolatedKeyFrame(interpolatedKeyTension);
+ EXPECT_TRUE(interpolatedKeyTension.Translation() ==
+ math::Vector3d(10, 20, 30));
+ EXPECT_TRUE(interpolatedKeyTension.Rotation() ==
+ math::Quaterniond(0.1, 0.2, 0.3));
+}
+
 /////////////////////////////////////////////////
 TEST_F(AnimationTest, NumericAnimation)
 {