HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

update bt tree params

Browse Source
This commit is contained in:
david
2025-10-22 11:03:25 +08:00
parent 30f7001bbc
commit 6e59457dd4
6 changed files with 136 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/brain/config/bt_carry_boxes.params.yaml
View File

@@ -23,7 +23,7 @@
frame_time_stamp: 0
data_array: [0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]
'
- name: s3_hand_pickup
@@ -43,7 +43,7 @@
frame_time_stamp: 0
data_array: [0.198632, 0.513333, 0.072526, -0.71159701, 0.13862565, -0.14251799, 0.67387035, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817]
data_array: [-0.198632, -0.513333, 0.072526, -0.13862565, -0.71159701, 0.67387035, 0.14251799, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817]
'
- name: s5_waist_bend_up
@@ -85,7 +85,7 @@
frame_time_stamp: 0
data_array: [0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]
data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.325695, 0.447487, 0.108462, -0.65995416, 0.33866696, -0.16590247, 0.64980117]
'
- name: s11_hand_release
@@ -105,7 +105,7 @@
frame_time_stamp: 0
data_array: [0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123, 0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241]
data_array: [-0.179387, -0.341708, -0.099286, -0.12851699, -0.88140507, 0.41951188, 0.17498077, 0.128250, 0.554955, 0.096719, -0.66087188, 0.14284399, -0.04440424, 0.73544015]
'
- name: s13_camera_take_photo
@@ -131,6 +131,6 @@
frame_time_stamp: 0
data_array: [0.123562, 0.555266, 0.062529, -0.75662638, 0.02276690, -0.14333775, 0.63753626, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394]
data_array: [-0.123562, -0.555266, 0.062529, -0.02276690, -0.75662638, 0.63753626, 0.14333775, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394]
'

2
src/brain/config/bt_carry_boxes.xml
View File

@@ -5,7 +5,7 @@
<RetryUntilSuccessful name="retry_all_action" num_attempts="5">
<Sequence>
<!-- <MoveHome_H name="s0_move_home" /> -->
<MoveWaist_H name="s1_waist_bend_down" /> <!--pitch w rad -->
<MoveWaist_H name="s1_waist_bend_down" />
<Arm_H name="s2_arm_stretch_out" />
<HandControl_H name="s3_hand_pickup" />
<Arm_H name="s4_arm_lift" />

2
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -76,7 +76,7 @@ private:
size_t total_sequence_success_ {0};
size_t total_sequence_failure_ {0};
rclcpp::TimerBase::SharedPtr stats_timer_;
double stats_log_interval_sec_ {60.0};
double stats_log_interval_sec_ {30.0};
std::mutex stats_mutex_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;

13
src/brain/src/cerebellum_node.cpp
View File

@@ -166,6 +166,11 @@ void CerebellumNode::ConfigureArmHooks()
const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == 0);
const std::string message = res.result ? std::string("action end") : std::string("无返回信息");
geometry_msgs::msg::Pose pose = res.result->pose;
RCLCPP_WARN(this->get_logger(), "[%s] 最终位置: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f",
skill_name.c_str(), pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (success) {
RCLCPP_INFO(this->get_logger(), "[%s] command_id %ld 完成: %s", skill_name.c_str(), res.result->command_id, message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
@@ -813,12 +818,12 @@ void CerebellumNode::SetupExecuteBtServer()
goal->epoch, goal->action_name.c_str(), goal->calls.size());
for (size_t i = 0; i < goal->calls.size(); ++i) {
const auto & c = goal->calls[i];
RCLCPP_INFO(this->get_logger(), " call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
RCLCPP_INFO(this->get_logger(), "call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
if (!c.payload_yaml.empty()) {
// Log payload content (trim if very long)
const std::string payload = c.payload_yaml.size() > 512 ? c.payload_yaml.substr(0, 512) + "..." : c.payload_yaml;
RCLCPP_INFO(this->get_logger(), " payload: %s", payload.c_str());
RCLCPP_INFO(this->get_logger(), "payload:{%s}", payload.c_str());
}
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -1145,8 +1150,8 @@ bool CerebellumNode::ExecuteActionSkill(
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
if (t1.joinable()) t1.join();
if (t2.joinable()) t2.join();
if (t1.joinable()) t1.join(); //RIGHT ARM
if (t2.joinable()) t2.join(); //LEFT ARM
if (!ok1 || !ok2) {
std::ostringstream oss;

2
src/brain/src/cerebrum_node.cpp
View File

@@ -108,7 +108,7 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
RegisterActionClient();
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
task_timer_ = this->create_wall_timer(30000ms, [this]() {CerebrumTask();});
task_timer_ = this->create_wall_timer(20000ms, [this]() {CerebrumTask();});
CreateServices();

119
src/scripts/rotate_pose.py Normal file
View File

@@ -0,0 +1,119 @@
#!/usr/bin/env python3
"""
rotate_pose.py
Small utility to rotate a 7-element pose (x,y,z,qx,qy,qz,qw or qw,qx,qy,qz)
around a given axis by a specified angle (degrees).
Usage examples:
./rotate_pose.py --pose 0.1 0.2 0.3 -0.6 0.1 -0.1 0.7 --axis z --angle 180 --order qxqyqzqw --frame world
python3 rotate_pose.py --pose 0.179387 0.341708 -0.099286 -0.88140507 0.12851699 -0.17498077 0.41951188 --axis z --angle 180 --order qxqyqzqw --frame world
Supported frames:
- world: apply rotation in world frame (q' = q_rot * q)
- body: apply rotation in body/local frame (q' = q * q_rot)
Supported quaternion order strings: "qxqyqzqw" (default) or "wxyz" (qw,qx,qy,qz)
"""
import argparse
import math
from typing import List, Tuple
def normalize_quat(q: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
x, y, z, w = q
n = math.sqrt(x * x + y * y + z * z + w * w)
if n == 0:
return (0.0, 0.0, 0.0, 1.0)
return (x / n, y / n, z / n, w / n)
def axis_angle_to_quat(axis: str, angle_deg: float) -> Tuple[float, float, float, float]:
a = math.radians(angle_deg)
ca = math.cos(a / 2.0)
sa = math.sin(a / 2.0)
ax = axis.lower()
if ax == 'x':
return (sa, 0.0, 0.0, ca)
if ax == 'y':
return (0.0, sa, 0.0, ca)
if ax == 'z':
return (0.0, 0.0, sa, ca)
raise ValueError('axis must be x,y or z')
def quat_mul(a: Tuple[float,float,float,float], b: Tuple[float,float,float,float]) -> Tuple[float,float,float,float]:
ax, ay, az, aw = a
bx, by, bz, bw = b
x = aw*bx + ax*bw + ay*bz - az*by
y = aw*by - ax*bz + ay*bw + az*bx
z = aw*bz + ax*by - ay*bx + az*bw
w = aw*bw - ax*bx - ay*by - az*bz
return (x, y, z, w)
def rotate_position(pos: Tuple[float,float,float], axis: str, angle_deg: float) -> Tuple[float,float,float]:
# For 180-degree rotations we can do simple sign flips but implement general rotation using quaternion
qrot = axis_angle_to_quat(axis, angle_deg)
# convert pos to quaternion (v,0)
px, py, pz = pos
p = (px, py, pz, 0.0)
qrot_n = normalize_quat(qrot)
qrot_conj = (-qrot_n[0], -qrot_n[1], -qrot_n[2], qrot_n[3])
tmp = quat_mul(qrot_n, p)
p_rot = quat_mul(tmp, qrot_conj)
return (p_rot[0], p_rot[1], p_rot[2])
def parse_pose(vals: List[float], order: str) -> Tuple[Tuple[float,float,float], Tuple[float,float,float,float]]:
if order == 'qxqyqzqw':
if len(vals) != 7:
raise ValueError('need 7 values for pose')
return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6])
if order == 'wxyz':
if len(vals) != 7:
raise ValueError('need 7 values for pose')
return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6]) if False else (vals[4], vals[5], vals[6], vals[3])
raise ValueError('unsupported order')
def format_pose(pos: Tuple[float,float,float], quat: Tuple[float,float,float,float], order: str) -> List[float]:
if order == 'qxqyqzqw':
return [pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]]
if order == 'wxyz':
# qw,qx,qy,qz
return [pos[0], pos[1], pos[2], quat[3], quat[0], quat[1], quat[2]]
raise ValueError('unsupported order')
def main():
p = argparse.ArgumentParser()
p.add_argument('--pose', nargs=7, type=float, required=True, help='pose values')
p.add_argument('--axis', choices=['x','y','z'], default='z')
p.add_argument('--angle', type=float, default=180.0)
p.add_argument('--order', choices=['qxqyqzqw','wxyz'], default='qxqyqzqw')
p.add_argument('--frame', choices=['world','body'], default='world')
args = p.parse_args()
pos, quat = parse_pose(list(args.pose), args.order)
# rotation quaternion
qrot = axis_angle_to_quat(args.axis, args.angle)
qrot = normalize_quat(qrot)
# apply rotation to position in world frame: pos' = R * pos
pos_rot = rotate_position(pos, args.axis, args.angle)
# apply rotation to orientation
if args.frame == 'world':
qnew = quat_mul(qrot, quat)
else:
qnew = quat_mul(quat, qrot)
qnew = normalize_quat(qnew)
out = format_pose(pos_rot, qnew, args.order)
# Print as numeric list without quotes
print('rotated_pose:', [float('{:.8f}'.format(x)) for x in out])
if __name__ == '__main__':
main()