Merge branch 'feature/offset_tuning' into develop

Author: nyxrobotics

.rosinstall

@@ -1,45 +1,48 @@
 - git:
-    local-name: gazebo_dc_motor
-    uri: [email protected]:nyxrobotics/gazebo_dc_motor.git
-    version: develop
-- git:
-    local-name: robotis/robotis_math
-    uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Math.git
-    version: master
-- git:
-    local-name: robotis/robotis_framework
-    uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Framework.git
-    version: master
-- git:
-    local-name: robotis/robotis_controller_msgs
-    uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Framework-msgs.git
-    version: master
-- git:
-    local-name: robotis/robotis_op3_tools
-    uri: [email protected]:sbgisen/ROBOTIS-OP3-Tools.git
-    version: feature/noetic
-- git:
-    local-name: robotis/robotis_op3_msgs
-    uri: [email protected]:ROBOTIS-GIT/ROBOTIS-OP3-msgs.git
-    version: master
-- git:
-    local-name: robotis/robotis_op3
-    uri: [email protected]:sbgisen/ROBOTIS-OP3.git
-    version: feature/noetic
-- git:
-    local-name: robotis/humanoid_navigation
-    uri: [email protected]:sbgisen/humanoid_navigation.git
-    version: feature/noetic
-- git:
-    local-name: robotis/humanoid_msgs
-    uri: [email protected]:ahornung/humanoid_msgs.git
-    version: master
-- git:
-    local-name: fdilink_ahrs
-    uri: [email protected]:sbgisen/fdilink_ahrs.git
-    version: master
-- git:
-    local-name: yolov8
-    uri: [email protected]:sbgisen/yolov8.git
-    version: develop
-
+      local-name: gazebo_dc_motor
+      uri: [email protected]:nyxrobotics/gazebo_dc_motor.git
+      version: develop
+- git:
+      local-name: robotis/robotis_math
+      uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Math.git
+      version: master
+- git:
+      local-name: robotis/robotis_framework
+      uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Framework.git
+      version: master
+- git:
+      local-name: robotis/robotis_controller_msgs
+      uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Framework-msgs.git
+      version: master
+- git:
+      local-name: robotis/robotis_op3_tools
+      uri: [email protected]:sbgisen/ROBOTIS-OP3-Tools.git
+      version: feature/noetic
+- git:
+      local-name: robotis/robotis_op3_msgs
+      uri: [email protected]:ROBOTIS-GIT/ROBOTIS-OP3-msgs.git
+      version: master
+- git:
+      local-name: robotis/robotis_op3
+      uri: [email protected]:sbgisen/ROBOTIS-OP3.git
+      version: feature/noetic
+- git:
+      local-name: robotis/robotis_optimization
+      uri: [email protected]:ROBOTIS-GIT/ROBOTIS-Optimization.git
+      version: master
+- git:
+      local-name: robotis/humanoid_navigation
+      uri: [email protected]:sbgisen/humanoid_navigation.git
+      version: feature/noetic
+- git:
+      local-name: robotis/humanoid_msgs
+      uri: [email protected]:ahornung/humanoid_msgs.git
+      version: master
+- git:
+      local-name: fdilink_ahrs
+      uri: [email protected]:sbgisen/fdilink_ahrs.git
+      version: master
+- git:
+      local-name: yolov8
+      uri: [email protected]:sbgisen/yolov8.git
+      version: develop

README.md

@@ -27,7 +27,23 @@ catkin build
 bash ../devel/setup.sh
 ```
 
+## RoboOne-Auto
+```bash
+roslaunch kuroko_bringup roboone_startup.launch
+```
+
 ## Gazebo
 ```bash
-roslaunch kuroko_description roboone_gazebo.launch
+roslaunch kuroko_bringup kuroko_bringup_gazebo.launch
+roslaunch kuroko_bringup manager.launch
+```
+
+- RonoOne
+```bash
+roslaunch kuroko_roboone roboone_auto.launch
+```
+
+- Walking GUI
+```bash
+roslaunch kuroko_walking_gui kuroko_walking_gui.launch
 ```

install_dependency.sh

@@ -38,6 +38,10 @@ done
 
 rosdep update
 rosdep install -r -y -i --from-paths .
+
+## Manually install some apt packages
+sudo apt install scilab
+
 catkin source
 catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Release
 roscd yolov8 && pipenv install

kuroko_action_module/CMakeLists.txt

@@ -34,7 +34,11 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME} src/action_module.cpp src/motion_files.cpp)
+add_library(
+  ${PROJECT_NAME}
+  src/action_module.cpp
+  src/animation_files.cpp
+)
 add_dependencies(
   ${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}

kuroko_action_module/config/joint_names.yaml

@@ -1,7 +1,6 @@
 #ifndef KUROKO_ACTION_MODULE_H_
 #define KUROKO_ACTION_MODULE_H_
 
-// Check if the C++ standard is 17 or later
 #if __cplusplus >= 201703L
 #include <filesystem>
 namespace fs = std::filesystem;
@@ -16,7 +15,6 @@ namespace fs = std::experimental::filesystem;
 #include <string>
 #include <vector>
 #include <boost/thread.hpp>
-#include <fstream>
 #include <ros/ros.h>
 #include <ros/package.h>
 #include <ros/callback_queue.h>
@@ -25,72 +23,74 @@ namespace fs = std::experimental::filesystem;
 #include "robotis_controller_msgs/SyncWriteItem.h"
 #include "std_msgs/String.h"
 #include "std_msgs/Int32.h"
+#include "trajectory_msgs/JointTrajectory.h"
 #include "op3_action_module_msgs/IsRunning.h"
 #include "op3_action_module_msgs/StartAction.h"
-#include "motion_files.h"
+#include "animation_files.h"
 
 namespace motion_control
 {
 class ActionModule : public robotis_framework::MotionModule, public robotis_framework::Singleton<ActionModule>
 {
 public:
   ActionModule();
-  virtual ~ActionModule();
+  ~ActionModule() override;
 
+  // ROS Framework Functions
   void initialize(const int control_cycle_msec, robotis_framework::Robot* robot) override;
   void process(std::map<std::string, robotis_framework::Dynamixel*> dxls,
                std::map<std::string, double> sensors) override;
 
-  void saveAllMotions(const std::string& directory);
-  void loadAllMotions(const std::string& directory);
+  void stop() override;
+  bool isRunning() override;
 
-  bool playMotionByName(const std::string& motion_name);
-  void brake();
   void onModuleEnable() override;
   void onModuleDisable() override;
-  bool isRunning() override;
-  void stop() override;
-  std::vector<std::string> getMotionNames();
-  void torqueOnAll();
-  void torqueOffAll();
 
 private:
+  std::string current_animation_name_;
+  int current_block_id_;
+  double time_in_frame_ = 0.0;
+  bool is_running_ = false;
+
   boost::thread queue_thread_;
   ros::Publisher status_msg_pub_;
   ros::Publisher done_msg_pub_;
   ros::Publisher sync_write_pub_;
 
   int control_cycle_msec_;
-
-  bool start_playing_requested_;
-  bool stop_playing_requested_;
-  bool action_module_enabled_;
-  bool send_next_frame_;
-
-  MotionFiles motion_files_;
-  MotionStatus motion_status_;
+  bool action_module_initialized_;
+  trajectory_msgs::JointTrajectory current_trajectory_;
+  size_t trajectory_index_ = 0;
+  ros::Time trajectory_start_time_;
+  bool start_playing_requested_ = false;
+  bool stop_playing_requested_ = false;
 
   std::map<std::string, int> joint_name_to_dxl_id_;
   std::map<int, std::string> dxl_id_to_joint_name_;
-  std::map<std::string, robotis_framework::DynamixelState*> action_result_;
   std::map<std::string, bool> action_joints_enable_;
+  std::vector<std::string> animation_joint_names_;
 
-  void queueThread();
-  void publishStatusMsg(unsigned int type, std::string msg);
-  void publishDoneMsg(std::string msg);
+  animation_system::Workspace workspace_;
 
+  // ROS Topic Callback Functions
   bool isRunningServiceCallback(op3_action_module_msgs::IsRunning::Request& req,
                                 op3_action_module_msgs::IsRunning::Response& res);
-  void motionNumberCallback(const std_msgs::Int32::ConstPtr& msg);
+  void animationNumberCallback(const std_msgs::Int32::ConstPtr& msg);
   void startActionCallback(const op3_action_module_msgs::StartAction::ConstPtr& msg);
-  void processMotionStep();
 
-  void loadConfigJointNames(const std::string& file_name);
-  void loadMotionYAML(const std::string& file_name, const std::string& motion_name);
-
-  std::vector<std::string> config_joint_names_;
+  // User functions
+  void queueThread();
+  void publishStatusMsg(unsigned int type, std::string msg);
+  void publishDoneMsg(std::string msg);
+  void getJointNames();
+  void torqueOnAll();
+  void torqueOffAll();
+  trajectory_msgs::JointTrajectory createJointTrajectory(const std::vector<animation_system::FrameData>& frames,
+                                                         const double control_cycle_msec);
+  std::vector<animation_system::FrameData> getFrameVector(const animation_system::AnimationData& animation_data);
 };
 
 }  // namespace motion_control
 
-#endif /* KUROKO_ACTION_MODULE_H_ */
+#endif  // KUROKO_ACTION_MODULE_H_

kuroko_action_module/include/kuroko_action_module/action_module.h

@@ -0,0 +1,98 @@
+#ifndef ANIMATION_FILES_H_
+#define ANIMATION_FILES_H_
+
+#include <string>
+#include <vector>
+#include <map>
+
+namespace animation_system
+{
+struct JointData
+{
+  double position = 0.0;
+  double speed_scale = 1.0;
+  bool enable = true;
+  std::vector<double> pid = { 0.0, 0.0, 0.0 };
+  std::string feedback;
+};
+
+struct FrameData
+{
+  double move_duration = 1.0;
+  double wait_duration = 0.0;
+  std::map<std::string, JointData> joints;  // <joint_name, JointData>
+};
+
+struct IfData
+{
+  std::string expression;
+  std::string condition;
+};
+
+struct SwitchData
+{
+  std::string expression;
+  std::string condition;
+  std::map<std::string, std::map<std::string, int>> cases;
+};
+
+struct AnimationBlock
+{
+  std::string type;
+  std::string filename;
+  int id;
+  std::vector<int> output_ids;
+};
+
+class AnimationData
+{
+public:
+  std::map<int, AnimationBlock> blocks;  // <block_id, AnimationBlock>
+  FrameData initial_frame_data;
+  std::map<std::string, FrameData> frames;              // <filename, FrameData>
+  std::map<std::string, IfData> if_conditions;          // <filename, IfData>
+  std::map<std::string, SwitchData> switch_conditions;  // <filename, SwitchData>
+
+  void setAnimationBlock(const int block_id, const AnimationBlock& block);
+  void removeAnimationBlock(const int block_id);
+  AnimationBlock getAnimationBlock(const int block_id) const;
+  std::vector<int> getBlockIds() const;
+
+  void setInitialFrameData(const FrameData& frame);
+  FrameData getInitialFrameData() const;
+
+  void setFrameData(const std::string& filename, const FrameData& frame_data);
+  void removeFrameData(const std::string& filename);
+  FrameData getFrameData(const std::string& filename) const;
+  std::vector<std::string> getFrameFilenames();
+
+  void setIfData(const std::string& filename, const IfData& if_data);
+  void removeIfData(const std::string& filename);
+  IfData getIfData(const std::string& filename);
+  std::vector<std::string> getIfFilenames();
+
+  void setSwitchData(const std::string& filename, const SwitchData& switch_data);
+  void removeSwitchData(const std::string& filename);
+  SwitchData getSwitchData(const std::string& filename);
+  std::vector<std::string> getSwitchFilenames();
+
+  int getStartBlockId() const;
+  int getBlockIdByName(const std::string& block_name) const;
+};
+
+class Workspace
+{
+public:
+  std::map<std::string, AnimationData> animations;  // <animation_name, AnimationData>
+  FrameData initial_pose_data;
+
+  void loadWorkspace(const std::string& workspace_path);
+  std::vector<std::string> getAnimationNames();
+  const AnimationData& getAnimationData(const std::string& name) const;
+  FrameData getInitialPoseData();
+  void setInitialPoseData(const FrameData& frame_data);
+};
+
+}  // namespace animation_system
+
+#endif  // ANIMATION_FILES_H_