Skip to content

Commit 3a9261c

Browse files
nyxroboticsnyxrobotics
committed
Merge branch 'feature/edit_motion_format' into develop
2 parents 295d5bc + e6deab3 commit 3a9261c

File tree

7 files changed

+255
-70
lines changed

7 files changed

+255
-70
lines changed

‎kuroko_action_module/include/kuroko_action_module/motion_files.h‎

Lines changed: 29 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,34 +1,53 @@
1-
21
#ifndef MOTION_FILES_H_
32
#define MOTION_FILES_H_
43

54
#include <trajectory_msgs/JointTrajectory.h>
5+
#include <string>
6+
#include <vector>
67

78
namespace motion_control
89
{
910
class MotionSection
1011
{
1112
public:
12-
std::string section_name;
13-
std::vector<std::string> next_sections;
14-
trajectory_msgs::JointTrajectory joint_trajectory;
15-
trajectory_msgs::JointTrajectory getSortedJointTrajectory(const std::vector<std::string> joint_names_in);
13+
std::string section_name; // Name of the current section
14+
std::string continue_section; // Section to proceed under normal conditions
15+
std::string stop_section; // Section to proceed when motion stops
16+
std::vector<std::string> jump_sections; // Sections to jump to based on user input
17+
trajectory_msgs::JointTrajectory joint_trajectory; // Joint trajectory for the current section
18+
19+
// Get sorted joint trajectory based on input joint order
20+
trajectory_msgs::JointTrajectory getSortedJointTrajectory(const std::vector<std::string>& joint_names_in);
1621
};
22+
1723
class MotionFile
1824
{
1925
public:
20-
std::string motion_name;
21-
std::vector<MotionSection> motion_sections;
22-
MotionSection getMotionSection(const std::string& section_name);
26+
std::string motion_name; // Name of the motion file
27+
std::vector<MotionSection> motion_sections; // List of motion sections
28+
29+
// Get a motion section by name
30+
MotionSection* getMotionSection(const std::string& section_name);
31+
32+
// Check if a motion section exists
33+
bool hasSection(const std::string& section_name);
2334
};
35+
2436
class MotionFiles
2537
{
2638
public:
27-
std::vector<MotionFile> motion_files;
28-
MotionFile getMotionFile(const std::string& motion_name);
39+
std::vector<MotionFile> motion_files; // List of all motion files
40+
41+
// Get a motion file by name
42+
MotionFile* getMotionFile(const std::string& motion_name);
43+
44+
// Check if a motion exists
2945
bool hasMotion(const std::string& motion_name);
46+
47+
// Get all motion names
3048
std::vector<std::string> getMotionNames();
3149
};
50+
3251
class MotionStatus
3352
{
3453
public:

‎kuroko_action_module/motion/motion00_wakeup_front.yaml‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
sections:
22
- section_name: start_section
3-
next_sections: []
3+
continue_section: ""
4+
stop_section: ""
5+
jump_sections: []
46
joint_trajectory:
57
joint_names:
68
- chest

‎kuroko_action_module/motion/motion01_wakeup_rear.yaml‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
sections:
22
- section_name: start_section
3-
next_sections: []
3+
continue_section: ""
4+
stop_section: ""
5+
jump_sections: []
46
joint_trajectory:
57
joint_names:
68
- chest

‎kuroko_action_module/motion/motion02_atk.yaml‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
sections:
22
- section_name: start_section
3-
next_sections: []
3+
continue_section: ""
4+
stop_section: ""
5+
jump_sections: []
46
joint_trajectory:
57
joint_names:
68
- chest

‎kuroko_action_module/motion/sample.yaml‎

Lines changed: 132 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
sections:
2+
- section_name: start_section
3+
continue_section: "loop_section" # Section to proceed after this one completes
4+
stop_section: "finish_section" # Section to proceed when stop condition (e.g., key release) is met
5+
jump_sections: [] # No additional jump sections
6+
joint_trajectory:
7+
joint_names:
8+
- chest
9+
- shoulder_r_pitch
10+
- shoulder_r_roll
11+
- elbow_r_front
12+
- elbow_r_rear
13+
- shoulder_l_pitch
14+
- shoulder_l_roll
15+
- elbow_l_front
16+
- elbow_l_rear
17+
- hip_r_roll
18+
- hip_r_pitch
19+
- thigh_r_active
20+
- shin_r_active
21+
- ankle_r_roll
22+
- ankle_r_yaw
23+
- hip_l_roll
24+
- hip_l_pitch
25+
- thigh_l_active
26+
- shin_l_active
27+
- ankle_l_roll
28+
- ankle_l_yaw
29+
points:
30+
- positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
31+
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
32+
accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
33+
time_from_start: 0.1
34+
35+
- section_name: loop_section
36+
continue_section: "loop_section" # This section loops back to itself
37+
stop_section: "finish_section" # Section to proceed when stop condition is met
38+
jump_sections: ["abort_section"] # Allows a jump to the abort section on specific input
39+
joint_trajectory:
40+
joint_names:
41+
- chest
42+
- shoulder_r_pitch
43+
- shoulder_r_roll
44+
- elbow_r_front
45+
- elbow_r_rear
46+
- shoulder_l_pitch
47+
- shoulder_l_roll
48+
- elbow_l_front
49+
- elbow_l_rear
50+
- hip_r_roll
51+
- hip_r_pitch
52+
- thigh_r_active
53+
- shin_r_active
54+
- ankle_r_roll
55+
- ankle_r_yaw
56+
- hip_l_roll
57+
- hip_l_pitch
58+
- thigh_l_active
59+
- shin_l_active
60+
- ankle_l_roll
61+
- ankle_l_yaw
62+
points:
63+
- positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
64+
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
65+
accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
66+
time_from_start: 0.1
67+
68+
- section_name: finish_section
69+
continue_section: "" # No further sections
70+
stop_section: "" # No further sections
71+
jump_sections: [] # No jumps
72+
joint_trajectory:
73+
joint_names:
74+
- chest
75+
- shoulder_r_pitch
76+
- shoulder_r_roll
77+
- elbow_r_front
78+
- elbow_r_rear
79+
- shoulder_l_pitch
80+
- shoulder_l_roll
81+
- elbow_l_front
82+
- elbow_l_rear
83+
- hip_r_roll
84+
- hip_r_pitch
85+
- thigh_r_active
86+
- shin_r_active
87+
- ankle_r_roll
88+
- ankle_r_yaw
89+
- hip_l_roll
90+
- hip_l_pitch
91+
- thigh_l_active
92+
- shin_l_active
93+
- ankle_l_roll
94+
- ankle_l_yaw
95+
points:
96+
- positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
97+
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
98+
accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
99+
time_from_start: 0.1
100+
101+
- section_name: abort_section
102+
continue_section: "" # No further sections
103+
stop_section: "" # No further sections
104+
jump_sections: [] # No jumps
105+
joint_trajectory:
106+
joint_names:
107+
- chest
108+
- shoulder_r_pitch
109+
- shoulder_r_roll
110+
- elbow_r_front
111+
- elbow_r_rear
112+
- shoulder_l_pitch
113+
- shoulder_l_roll
114+
- elbow_l_front
115+
- elbow_l_rear
116+
- hip_r_roll
117+
- hip_r_pitch
118+
- thigh_r_active
119+
- shin_r_active
120+
- ankle_r_roll
121+
- ankle_r_yaw
122+
- hip_l_roll
123+
- hip_l_pitch
124+
- thigh_l_active
125+
- shin_l_active
126+
- ankle_l_roll
127+
- ankle_l_yaw
128+
points:
129+
- positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
130+
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
131+
accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
132+
time_from_start: 0.1

‎kuroko_action_module/src/action_module.cpp‎

Lines changed: 42 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -247,9 +247,20 @@ void ActionModule::process(std::map<std::string, robotis_framework::Dynamixel*>
247247
if (send_next_frame_)
248248
{
249249
ROS_INFO_STREAM("[ActionModule] Sending next frame: " << motion_status_.current_frame_in_section);
250-
trajectory_msgs::JointTrajectory sorted_trajectory = motion_files_.getMotionFile(motion_status_.current_motion_name)
251-
.getMotionSection(motion_status_.current_section_name)
252-
.getSortedJointTrajectory(config_joint_names_);
250+
MotionFile* motion_file = motion_files_.getMotionFile(motion_status_.current_motion_name);
251+
if (motion_file == nullptr)
252+
{
253+
ROS_ERROR_STREAM("[ActionModule] process: motion file not found");
254+
return;
255+
}
256+
MotionSection* motion_section = motion_file->getMotionSection(motion_status_.current_section_name);
257+
if (motion_section == nullptr)
258+
{
259+
ROS_ERROR_STREAM("[ActionModule] process: motion section not found");
260+
return;
261+
}
262+
263+
trajectory_msgs::JointTrajectory sorted_trajectory = motion_section->getSortedJointTrajectory(config_joint_names_);
253264
for (int i = 0; i < sorted_trajectory.joint_names.size(); i++)
254265
{
255266
std::string joint_name = sorted_trajectory.joint_names[i];
@@ -285,7 +296,7 @@ bool ActionModule::playMotionByName(const std::string& motion_name)
285296

286297
motion_status_.next_motion_name = motion_name; // Set current_motion_ to the selected motion
287298
motion_status_.start_requested = true; // Set start_requested to true
288-
MotionFile motion_file = motion_files_.getMotionFile(motion_name);
299+
MotionFile motion_file = *motion_files_.getMotionFile(motion_name);
289300
return true;
290301
}
291302

@@ -317,10 +328,10 @@ void ActionModule::saveAllMotions(const std::string& directory)
317328
YAML::Node section_node;
318329
section_node["section_name"] = section.section_name;
319330

320-
// Add next_sections if available
321-
if (!section.next_sections.empty())
331+
// Add stop_section if available
332+
if (!section.stop_section.empty())
322333
{
323-
section_node["next_sections"] = section.next_sections;
334+
section_node["stop_section"] = section.stop_section;
324335
}
325336

326337
YAML::Node joint_trajectory_node;
@@ -397,10 +408,10 @@ void ActionModule::loadMotionYAML(const std::string& file_name, const std::strin
397408
motion_control::MotionSection motion_section;
398409
motion_section.section_name = section["section_name"].as<std::string>();
399410

400-
// Load next_sections
401-
if (section["next_sections"])
411+
// Load stop_section
412+
if (section["stop_section"])
402413
{
403-
motion_section.next_sections = section["next_sections"].as<std::vector<std::string>>();
414+
motion_section.stop_section = section["stop_section"].as<std::string>();
404415
}
405416

406417
// Load joint trajectory
@@ -478,9 +489,20 @@ void ActionModule::processMotionStep()
478489
<< " not found");
479490
return;
480491
}
492+
MotionFile* motion_file = motion_files_.getMotionFile(motion_status_.current_motion_name);
493+
if (motion_file == nullptr)
494+
{
495+
ROS_ERROR_STREAM("[ActionModule] processMotionStep: motion file not found");
496+
return;
497+
}
498+
std::vector<MotionSection> motion_sections = motion_file->motion_sections;
499+
if (motion_sections.empty())
500+
{
501+
ROS_ERROR_STREAM("[ActionModule] processMotionStep: motion sections not found");
502+
return;
503+
}
481504

482-
motion_status_.current_section_name =
483-
motion_files_.getMotionFile(motion_status_.current_motion_name).motion_sections[0].section_name;
505+
motion_status_.current_section_name = motion_sections[0].section_name;
484506
motion_status_.current_frame_in_section = 0;
485507
motion_status_.is_running = true;
486508
motion_status_.start_requested = false;
@@ -495,9 +517,14 @@ void ActionModule::processMotionStep()
495517
}
496518

497519
// Get the current section's joint trajectory points
498-
const auto& points = motion_files_.getMotionFile(motion_status_.current_motion_name)
499-
.getMotionSection(motion_status_.current_section_name)
500-
.joint_trajectory.points;
520+
MotionSection* current_section = motion_files_.getMotionFile(motion_status_.current_motion_name)
521+
->getMotionSection(motion_status_.current_section_name);
522+
if (current_section == nullptr)
523+
{
524+
ROS_ERROR_STREAM("[ActionModule] processMotionStep: current section not found");
525+
return;
526+
}
527+
const auto& points = current_section->joint_trajectory.points;
501528

502529
// Skip interpolation for the first frame, directly move to the next frame
503530
if (motion_status_.current_frame_in_section == 0)

0 commit comments

Comments
 (0)