Initialize MotionData using provided initial values

include/abb_egm_rws_managers/utilities.h

@@ -96,15 +96,62 @@ struct ABB_EGM_RWS_MANAGERS_EXPORT Constants
  */
 MechanicalUnitGroup findMechanicalUnitGroup(const std::string& name, const RobotControllerDescription& description);
 
+/**
+ * \brief A data structure to bundle the initial states & commands for position, velocity and effort of a joint.
+ */
+struct InitialJointValue
+{
+  /**
+   * \brief Initial position state.
+   */
+  double position_state;
+
+  /**
+   * \brief Initial position command.
+   */
+  double position_command;
+
+  /**
+   * \brief Initial velocity state.
+   */
+  double velocity_state;
+
+  /**
+   * \brief Initial velocity command.
+   */
+  double velocity_command;
+
+  /**
+   * \brief Initial effort state.
+   */
+  double effort_state;
+
+  /**
+   * \brief Default constructor.
+   */
+  InitialJointValue()
+  : position_state(0.0),
+    position_command(0.0),
+    velocity_state(0.0),
+    velocity_command(0.0),
+    effort_state(0.0)
+  {}
+
+};
+
 /**
  * \brief Initializes a motion data representation based on the description of a robot controller.
  *
  * \param motion_data representation to initialize.
  * \param description of the robot controller.
+ * \param initial_values for the joints of the robot.
  *
  * \throw std::runtime_error if the initialization failed (e.g. number of axes are not equal to number of joints).
  */
-void initializeMotionData(MotionData& motion_data, const RobotControllerDescription& description);
+void initializeMotionData(
+  MotionData& motion_data,
+  const RobotControllerDescription& description,
+  const std::vector<InitialJointValue>& joint_values = {});
 
 /**
  * \brief Creates a summary text of a robot controller description.

src/utilities.cpp

@@ -76,7 +76,10 @@ MechanicalUnitGroup findMechanicalUnitGroup(const std::string& name, const Robot
   return mug;
 }
 
-void initializeMotionData(MotionData& motion_data, const RobotControllerDescription& description)
+void initializeMotionData(
+  MotionData& motion_data,
+  const RobotControllerDescription& description,
+  const std::vector<InitialJointValue>& joint_values)
 {
   const auto& rw_version{description.header().robot_ware_version()};
 
@@ -122,19 +125,27 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript
     }
 
     // Add joint information.
+    std::size_t joint_count = 0;
     for(const auto& standardized_joint : unit.standardized_joints())
     {
       MotionData::Joint motion_joint{};
       motion_joint.name = standardized_joint.standardized_name();
       motion_joint.rotational = standardized_joint.rotating_move();
       motion_joint.lower_limit = standardized_joint.lower_joint_bound();
       motion_joint.upper_limit = standardized_joint.upper_joint_bound();
-      motion_joint.state.position = 0.0;
-      motion_joint.state.velocity = 0.0;
-      motion_joint.state.effort = 0.0;
-      motion_joint.command.position = 0.0;
-      motion_joint.command.velocity = 0.0;
+      InitialJointValue initial_value;
+      // Use InitialJointValue if provided.
+      if (joint_count < joint_values.size())
+      {
+        initial_value = joint_values.at(joint_count);
+      }
+      motion_joint.state.position = initial_value.position_state;
+      motion_joint.state.velocity = initial_value.velocity_state;
+      motion_joint.state.effort = initial_value.effort_state;
+      motion_joint.command.position = initial_value.position_command;
+      motion_joint.command.velocity = initial_value.velocity_command;
       motion_unit.joints.push_back(motion_joint);
+      ++joint_count;
     }
 
     return motion_unit;