feat: add set_point(s) functionality

source/state_representation/include/state_representation/trajectories/CartesianTrajectory.hpp

@@ -48,6 +48,28 @@ class CartesianTrajectory : public TrajectoryBase<Eigen::VectorXd> {
    */
   CartesianState get_point(unsigned int index) const;
 
+  /**
+   * @brief Set the trajectory point at given index
+   * @param index the index
+   * @param point the new point
+   * @param new_time the new time
+   * @return Success of the operation
+   */
+  template<typename DurationT>
+  bool
+  set_point(unsigned int index, const CartesianState& point, const std::chrono::duration<int64_t, DurationT>& new_time);
+
+  /**
+   * @brief Set the trajectory point at given index
+   * @param points vector of new points
+   * @param new_time vector of new times
+   * @return Success of the operation
+   */
+  template<typename DurationT>
+  bool set_points(
+      const std::vector<CartesianState>& points,
+      const std::vector<std::chrono::duration<int64_t, DurationT>>& new_times);
+
   /**
    * @brief Operator overload for returning a single trajectory point and
    * corresponding time
@@ -73,6 +95,9 @@ class CartesianTrajectory : public TrajectoryBase<Eigen::VectorXd> {
 template<typename DurationT>
 inline bool CartesianTrajectory::add_point(
     const CartesianState& new_point, const std::chrono::duration<int64_t, DurationT>& new_time) {
+  if (new_point.is_empty()) {
+    return false;
+  }
   if (this->get_size() > 0) {
     if (new_point.get_reference_frame() != reference_frame_) {
       return false;
@@ -87,6 +112,9 @@ inline bool CartesianTrajectory::add_point(
 template<typename DurationT>
 inline bool CartesianTrajectory::insert_point(
     const CartesianState& new_point, const std::chrono::duration<int64_t, DurationT>& new_time, int pos) {
+  if (new_point.is_empty()) {
+    return false;
+  }
   if (this->get_size() > 0) {
     if (new_point.get_reference_frame() != reference_frame_) {
       return false;
@@ -98,4 +126,40 @@ inline bool CartesianTrajectory::insert_point(
   return true;
 }
 
+template<typename DurationT>
+inline bool CartesianTrajectory::set_point(
+    unsigned int index, const CartesianState& point, const std::chrono::duration<int64_t, DurationT>& new_time) {
+  if (point.is_empty()) {
+    return false;
+  }
+  if (point.get_reference_frame() != reference_frame_) {
+    return false;
+  }
+  return this->TrajectoryBase<Eigen::VectorXd>::set_point(index, point.data(), new_time);
+}
+
+template<typename DurationT>
+inline bool CartesianTrajectory::set_points(
+    const std::vector<CartesianState>& points,
+    const std::vector<std::chrono::duration<int64_t, DurationT>>& new_times) {
+  if (points.empty()) {
+    return false;
+  }
+  std::string candidate_reference_frame = points[0].get_reference_frame();
+
+  std::vector<Eigen::VectorXd> data;
+  for (const auto& point : points) {
+    if (point.is_empty() || (point.get_reference_frame() != candidate_reference_frame)) {
+      return false;
+    }
+    data.push_back(point.data());
+  }
+  if (this->TrajectoryBase<Eigen::VectorXd>::set_points(data, new_times)) {
+    reference_frame_ = candidate_reference_frame;
+    return true;
+  } else {
+    return false;
+  }
+}
+
 }// namespace state_representation

source/state_representation/include/state_representation/trajectories/JointTrajectory.hpp

@@ -52,6 +52,27 @@ class JointTrajectory : public TrajectoryBase<Eigen::VectorXd> {
    */
   const JointState get_point(unsigned int index) const;
 
+  /**
+   * @brief Set the trajectory point at given index
+   * @param index the index
+   * @param point the new point
+   * @param new_time the new time
+   * @return Success of the operation
+   */
+  template<typename DurationT>
+  bool
+  set_point(unsigned int index, const JointState& point, const std::chrono::duration<int64_t, DurationT>& new_time);
+
+  /**
+   * @brief Set the trajectory point at given index
+   * @param points vector of new points
+   * @param new_time vector of new times
+   * @return Success of the operation
+   */
+  template<typename DurationT>
+  bool set_points(
+      const std::vector<JointState>& points, const std::vector<std::chrono::duration<int64_t, DurationT>>& new_times);
+
   /**
    * @brief Operator overload for returning a single trajectory point and
    * corresponding time
@@ -77,6 +98,9 @@ class JointTrajectory : public TrajectoryBase<Eigen::VectorXd> {
 template<typename DurationT>
 inline bool
 JointTrajectory::add_point(const JointState& new_point, const std::chrono::duration<int64_t, DurationT>& new_time) {
+  if (new_point.is_empty()) {
+    return false;
+  }
   if (this->get_size() == 0) {
     this->joint_names_ = new_point.get_names();
     this->robot_name_ = new_point.get_name();
@@ -90,6 +114,9 @@ JointTrajectory::add_point(const JointState& new_point, const std::chrono::durat
 template<typename DurationT>
 inline bool JointTrajectory::insert_point(
     const JointState& new_point, const std::chrono::duration<int64_t, DurationT>& new_time, int pos) {
+  if (new_point.is_empty()) {
+    return false;
+  }
   if (this->get_size() == 0) {
     this->joint_names_ = new_point.get_names();
     this->robot_name_ = new_point.get_name();
@@ -99,4 +126,39 @@ inline bool JointTrajectory::insert_point(
   this->TrajectoryBase<Eigen::VectorXd>::insert_point(new_point.data(), new_time, pos);
   return true;
 }
+
+template<typename DurationT>
+inline bool JointTrajectory::set_point(
+    unsigned int index, const JointState& point, const std::chrono::duration<int64_t, DurationT>& new_time) {
+  if (point.is_empty() || (point.get_names() != this->joint_names_) || (point.get_name() != this->robot_name_)) {
+    return false;
+  }
+  return this->TrajectoryBase<Eigen::VectorXd>::set_point(index, point.data(), new_time);
+}
+
+template<typename DurationT>
+inline bool JointTrajectory::set_points(
+    const std::vector<JointState>& points, const std::vector<std::chrono::duration<int64_t, DurationT>>& new_times) {
+  if (points.empty() || points[0].is_empty()) {
+    return false;
+  }
+  auto candidate_robot_name = points[0].get_name();
+  auto candidate_joint_names = points[0].get_names();
+
+  std::vector<Eigen::VectorXd> data;
+  for (const auto& point : points) {
+    if (point.is_empty() || (point.get_names() != candidate_joint_names)
+        || (point.get_name() != candidate_robot_name)) {
+      return false;
+    }
+    data.push_back(point.data());
+  }
+  if (this->TrajectoryBase<Eigen::VectorXd>::set_points(data, new_times)) {
+    this->joint_names_ = candidate_joint_names;
+    this->robot_name_ = candidate_robot_name;
+    return true;
+  } else {
+    return false;
+  }
+}
 }// namespace state_representation

source/state_representation/include/state_representation/trajectories/TrajectoryBase.hpp

@@ -81,6 +81,27 @@ class TrajectoryBase : public State {
    */
   TrajectoryT& get_point(unsigned int index);
 
+  /**
+   * @brief Set the trajectory point at given index
+   * @param index the index
+   * @param point the new point
+   * @param new_time the new time
+   * @return Success of the operation
+   */
+  template<typename DurationT>
+  bool
+  set_point(unsigned int index, const TrajectoryT& point, const std::chrono::duration<int64_t, DurationT>& new_time);
+
+  /**
+   * @brief Set the trajectory point at given index
+   * @param points vector of new points
+   * @param new_time vector of new times
+   * @return Success of the operation
+   */
+  template<typename DurationT>
+  bool set_points(
+      const std::vector<TrajectoryT>& points, const std::vector<std::chrono::duration<int64_t, DurationT>>& new_times);
+
   /**
    * @brief Operator overload for returning a single trajectory point and
    * corresponding time
@@ -95,27 +116,27 @@ class TrajectoryBase : public State {
 };
 
 template<typename TrajectoryT>
-TrajectoryBase<TrajectoryT>::TrajectoryBase() : State() {
+inline TrajectoryBase<TrajectoryT>::TrajectoryBase() : State() {
   this->set_type(StateType::NONE);
   this->reset();
 }
 
 template<typename TrajectoryT>
-TrajectoryBase<TrajectoryT>::TrajectoryBase(const std::string& name) : State(name) {
+inline TrajectoryBase<TrajectoryT>::TrajectoryBase(const std::string& name) : State(name) {
   this->set_type(StateType::NONE);
   this->reset();
 }
 
 template<typename TrajectoryT>
-void TrajectoryBase<TrajectoryT>::reset() {
+inline void TrajectoryBase<TrajectoryT>::reset() {
   this->State::reset();
   this->points_.clear();
   this->times_.clear();
 }
 
 template<typename TrajectoryT>
 template<typename DurationT>
-void TrajectoryBase<TrajectoryT>::add_point(
+inline void TrajectoryBase<TrajectoryT>::add_point(
     const TrajectoryT& new_point, const std::chrono::duration<int64_t, DurationT>& new_time) {
   this->set_empty(false);
   this->points_.push_back(new_point);
@@ -130,7 +151,7 @@ void TrajectoryBase<TrajectoryT>::add_point(
 
 template<typename TrajectoryT>
 template<typename DurationT>
-void TrajectoryBase<TrajectoryT>::insert_point(
+inline void TrajectoryBase<TrajectoryT>::insert_point(
     const TrajectoryT& new_point, const std::chrono::duration<int64_t, DurationT>& new_time, int pos) {
   this->set_empty(false);
 
@@ -150,7 +171,7 @@ void TrajectoryBase<TrajectoryT>::insert_point(
 }
 
 template<typename TrajectoryT>
-void TrajectoryBase<TrajectoryT>::delete_point() {
+inline void TrajectoryBase<TrajectoryT>::delete_point() {
   this->set_empty(false);
   if (!this->points_.empty()) {
     this->points_.pop_back();
@@ -161,7 +182,7 @@ void TrajectoryBase<TrajectoryT>::delete_point() {
 }
 
 template<typename TrajectoryT>
-void TrajectoryBase<TrajectoryT>::clear() {
+inline void TrajectoryBase<TrajectoryT>::clear() {
   this->points_.clear();
   this->times_.clear();
 }
@@ -172,32 +193,66 @@ inline const std::deque<TrajectoryT>& TrajectoryBase<TrajectoryT>::get_points()
 }
 
 template<typename TrajectoryT>
-const TrajectoryT& TrajectoryBase<TrajectoryT>::get_point(unsigned int index) const {
+inline const TrajectoryT& TrajectoryBase<TrajectoryT>::get_point(unsigned int index) const {
   return this->points_[index];
 }
 
 template<typename TrajectoryT>
-TrajectoryT& TrajectoryBase<TrajectoryT>::get_point(unsigned int index) {
+inline TrajectoryT& TrajectoryBase<TrajectoryT>::get_point(unsigned int index) {
   return this->points_[index];
 }
 
+template<typename TrajectoryT>
+template<typename DurationT>
+inline bool TrajectoryBase<TrajectoryT>::set_point(
+    unsigned int index, const TrajectoryT& point, const std::chrono::duration<int64_t, DurationT>& new_time) {
+  if (index < this->points_.size()) {
+    this->points_[index] = point;
+    if (index == 0) {
+      this->times_[index] = new_time;
+    } else {
+      this->times_[index] = this->times_[index - 1] + new_time;
+    }
+    for (unsigned int i = index + 1; i < this->points_.size(); ++i) {
+      this->times_[i] += this->times_[index - 1];
+    }
+    return true;
+  }
+  return false;
+}
+
+template<typename TrajectoryT>
+template<typename DurationT>
+inline bool TrajectoryBase<TrajectoryT>::set_points(
+    const std::vector<TrajectoryT>& points, const std::vector<std::chrono::duration<int64_t, DurationT>>& new_times) {
+  if (points.size() != new_times.size()) {
+    return false;
+  }
+  this->clear();
+  for (unsigned int i = 0; i < points.size(); ++i) {
+    this->add_point(points[i], new_times[i]);
+  }
+  return true;
+}
+
 template<typename TrajectoryT>
 inline const std::deque<std::chrono::nanoseconds>& TrajectoryBase<TrajectoryT>::get_times() const {
   return this->times_;
 }
 
 template<typename TrajectoryT>
-int TrajectoryBase<TrajectoryT>::get_size() const {
+inline int TrajectoryBase<TrajectoryT>::get_size() const {
   return this->points_.size();
 }
 
 template<typename TrajectoryT>
-const std::pair<TrajectoryT, std::chrono::nanoseconds> TrajectoryBase<TrajectoryT>::operator[](unsigned int idx) const {
+inline const std::pair<TrajectoryT, std::chrono::nanoseconds>
+TrajectoryBase<TrajectoryT>::operator[](unsigned int idx) const {
   return std::make_pair(this->points_[idx], this->times_[idx]);
 }
 
 template<typename TrajectoryT>
-std::pair<TrajectoryT, std::chrono::nanoseconds> TrajectoryBase<TrajectoryT>::operator[](unsigned int idx) {
+inline std::pair<TrajectoryT, std::chrono::nanoseconds> TrajectoryBase<TrajectoryT>::operator[](unsigned int idx) {
   this->set_empty(false);
   return std::make_pair(this->points_[idx], this->times_[idx]);
 }