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

support left arm

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-08 16:10:49 +08:00
parent 5c3994da9d
commit 68563e2162
7 changed files with 396 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -33,7 +33,7 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
data_array: [85, 25, 90, 0, 68, 90, 95, -25, -90, 0, -68, 90]
'
- name: s1_wheel_move_to_origin_pickup_position
@@ -53,7 +53,7 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 60.27, -20.97, -87.36, 5.58, -68.56, 58.21]
data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
'
- name: s2_top_camera_vision_recg
@@ -141,7 +141,7 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
data_array: [85, 25, 90, 0, 68, 90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
'
- name: s6_move_waist_turn_left_90
@@ -167,7 +167,7 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
data_array: [85, 25, 90, 0, 68, 90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s7_right_arm_put_down_box
@@ -181,7 +181,7 @@
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
data_array: [85, 25, 90, 0, 68, 90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
'
- name: s8_gripper_open

20
src/brain/config/bt_vision_grasp.xml
View File

@@ -49,18 +49,12 @@
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
<GripperCmd_H name="s1_gripper_open" />
<GripperCmd0_H name="s1_gripper_open" />
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
</Parallel>
<!-- <RetryUntilSuccessful name="retry_camera_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful> -->
<SubTree ID="vision_top_cam_subtree"/>
<Arm_H name="s2_right_arm_cam_take_photo" />
<!-- <RetryUntilSuccessful name="retry_camera_vision_recg_2" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful> -->
<SubTree ID="vision_hand_cam_subtree"/>
</Sequence>
</RetryUntilSuccessful>
@@ -89,7 +83,7 @@
<Sequence>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd_H name="s4_gripper_close" />
<GripperCmd0_H name="s4_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
</Sequence>
@@ -107,7 +101,7 @@
</Parallel> -->
<Arm_H name="s7_right_arm_grasp_box" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd_H name="s8_gripper_open" />
<GripperCmd0_H name="s8_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<Parallel name="parallel_action_3">
@@ -123,7 +117,7 @@
<Sequence name="recovery_root">
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<GripperCmd0_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
@@ -135,7 +129,7 @@
<Sequence name="vision_recovery_root">
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd_H name="s14_gripper_open" /> -->
<!-- <GripperCmd0_H name="s14_gripper_open" /> -->
<Arm_H name="s14_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
@@ -147,7 +141,7 @@
<Sequence name="grasp_recovery_root">
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
<Sequence>
<GripperCmd_H name="s15_gripper_open" />
<GripperCmd0_H name="s15_gripper_open" />
<Arm_H name="s15_arm_pre_grasp" />
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
@@ -160,7 +154,7 @@
<Sequence name="putdown_recovery_root">
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
<Sequence>
<GripperCmd_H name="s16_gripper_open" />
<GripperCmd0_H name="s16_gripper_open" />
<Arm_H name="s16_arm_pre_grasp" />
<Arm_H name="s16_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />

108
src/brain/config/bt_vision_grasp_dual_arm.params.yaml
View File

@@ -2,17 +2,7 @@
params: '{}
'
- name: retry_vision_hand
params: '{}
'
- name: s1_move_waist_turn_right_90
params: &waist_turn_right_90 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s1_gripper_open
- name: s1_right_hand_gripper_open
params: &gripper_open 'loc: 0
speed: 150
@@ -22,12 +12,10 @@
mode: 2
'
- name: s1_right_hand_gripper_open
params: *gripper_open
- name: s1_left_hand_gripper_open
params: *gripper_open
- name: s1_right_arm_initial
params: 'body_id: 1
params: &arm_inittial_right 'body_id: 1
data_type: 2
@@ -41,7 +29,7 @@
'
- name: s1_left_arm_initial
params: 'body_id: 0
params: &arm_inittial_left 'body_id: 0
data_type: 2
@@ -55,7 +43,7 @@
'
- name: s1_right_arm_pre_cam_take_photo
params: &pre_cam_take_photo_right 'body_id: 1
params: 'body_id: 1
data_type: 2
@@ -69,7 +57,7 @@
'
- name: s1_left_arm_pre_cam_take_photo
params: &pre_cam_take_photo_left 'body_id: 0
params: 'body_id: 0
data_type: 2
@@ -82,10 +70,34 @@
data_array: [120, 20, 80, 0, 80, 120, 0, 0, 0, 0, 0, 0]
'
- name: s2_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s2_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s2_right_arm_cam_take_photo
params: 'body_id: 1
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s2_left_arm_cam_take_photo
params: 'body_id: 0
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s2_top_camera_vision_recg
params: 'camera_position: top
@@ -286,26 +298,26 @@
params: *grasp_box_s7_s9_right
- name: s9_left_arm_grasp_box
params: *grasp_box_s7_s9_left
- name: s11_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s11_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s11_right_arm_initial
params: *arm_inittial_right
- name: s11_left_arm_initial
params: *arm_inittial_left
- name: s12_right_hand_gripper_open
params: *gripper_open
- name: s12_left_hand_gripper_open
params: *gripper_open
- name: s13_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s13_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s13_right_arm_initial
params: *arm_inittial_right
- name: s13_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_overall
params: '{}
'
- name: s14_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s14_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s14_right_arm_initial
params: *arm_inittial_right
- name: s14_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_vision
params: '{}
@@ -326,9 +338,27 @@
params: *pre_grasp_right
- name: s15_left_arm_pre_grasp
params: *pre_grasp_left
- name: s15_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s15_left_arm_pre_cam_take_photo
params: *pre_cam_take_photo_left
- name: s16_right_arm_pre_cam_take_photo
params: *pre_cam_take_photo_right
- name: s15_right_arm_initial
params: *arm_inittial_right
- name: s15_left_arm_initial
params: *arm_inittial_left
- name: s16_right_arm_initial
params: *arm_inittial_right
- name: s16_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_arm_vision
params: '{}
'
- name: s17_right_arm_initial
params: *arm_inittial_right
- name: s17_left_arm_initial
params: *arm_inittial_left
- name: clear_done_instances_left_arm_vision
params: '{}
'
- name: clear_done_instances_right_arm_vision
params: '{}
'

113
src/brain/config/bt_vision_grasp_dual_arm.xml
View File

@@ -1,6 +1,7 @@
<root BTCPP_format="4" main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<Script code="right_arm_done:=false; left_arm_done:=false" />
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Fallback name="fallback_main_action">
<Sequence name="complete_pick_and_place">
@@ -12,36 +13,60 @@
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Phase 2: Independent Dual Arm Operation -->
<!-- Asynchronous execution of both arms -->
<Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="1">
<Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
<!-- Right Arm Branch -->
<Sequence name="right_arm_branch">
<Fallback name="right_arm_process_with_recovery">
<Sequence name="right_arm_process">
<SubTree ID="right_arm_hand_vision_subtree"/>
<SubTree ID="right_arm_grasp_subtree"/>
<SubTree ID="right_arm_putdown_subtree"/>
</Sequence>
<Fallback>
<ScriptCondition code="right_arm_done==true" />
<Sequence>
<SubTree ID="right_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
<Fallback name="right_arm_vision_process_with_recovery">
<SubTree ID="right_arm_hand_vision_subtree"/>
<Sequence>
<SubTree ID="right_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Fallback name="right_arm_process_with_recovery">
<Sequence name="right_arm_process">
<SubTree ID="right_arm_grasp_subtree"/>
<SubTree ID="right_arm_putdown_subtree"/>
</Sequence>
<Sequence>
<SubTree ID="right_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Script code="right_arm_done:=true" />
</Sequence>
</Fallback>
</Sequence>
<!-- Left Arm Branch -->
<Sequence name="left_arm_branch">
<Fallback name="left_arm_process_with_recovery">
<Sequence name="left_arm_process">
<SubTree ID="left_arm_hand_vision_subtree"/>
<SubTree ID="left_arm_grasp_subtree"/>
<SubTree ID="left_arm_putdown_subtree"/>
</Sequence>
<Fallback>
<ScriptCondition code="left_arm_done==true" />
<Sequence>
<SubTree ID="left_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
<Fallback name="left_arm_vision_process_with_recovery">
<SubTree ID="left_arm_hand_vision_subtree"/>
<Sequence>
<SubTree ID="left_arm_vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Fallback name="left_arm_process_with_recovery">
<Sequence name="left_arm_process">
<SubTree ID="left_arm_grasp_subtree"/>
<SubTree ID="left_arm_putdown_subtree"/>
</Sequence>
<Sequence>
<SubTree ID="left_arm_grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<Script code="left_arm_done:=true" />
</Sequence>
</Fallback>
</Sequence>
@@ -72,9 +97,9 @@
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
<!-- Move both arms to positions where their hand cameras can see potential objects -->
<Parallel name="arm_pre_cam_take_photo1">
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
<Arm_H name="s2_left_arm_pre_cam_take_photo" />
<Parallel name="arm_cam_take_photo1">
<Arm_H name="s2_right_arm_cam_take_photo" />
<Arm_H name="s2_left_arm_cam_take_photo" />
</Parallel>
</Sequence>
</BehaviorTree>
@@ -110,7 +135,7 @@
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd0_H name="s8_right_hand_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
<Arm_H name="s11_right_arm_initial" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -147,7 +172,7 @@
<Arm_H name="s7_left_arm_put_down_box" />
<GripperCmd1_H name="s8_left_hand_gripper_open" />
<Arm_H name="s9_left_arm_grasp_box" />
<Arm_H name="s11_left_arm_pre_cam_take_photo" />
<Arm_H name="s11_left_arm_initial" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -159,8 +184,8 @@
<Parallel name="overall_recovery_parallel">
<GripperCmd0_H name="s12_right_hand_gripper_open" />
<GripperCmd1_H name="s12_left_hand_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<Arm_H name="s13_left_arm_pre_cam_take_photo" />
<Arm_H name="s13_right_arm_initial" />
<Arm_H name="s13_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
@@ -168,9 +193,9 @@
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<Parallel name="arm_pre_cam_take_photo2">
<Arm_H name="s14_right_arm_pre_cam_take_photo" />
<Arm_H name="s14_left_arm_pre_cam_take_photo" />
<Parallel name="arm_initial2">
<Arm_H name="s14_right_arm_initial" />
<Arm_H name="s14_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
@@ -180,7 +205,7 @@
<Sequence name="right_grasp_recovery_root">
<GripperCmd0_H name="s15_right_hand_gripper_open" />
<Arm_H name="s15_right_arm_pre_grasp" />
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
<Arm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
</Sequence>
</BehaviorTree>
@@ -189,8 +214,36 @@
<Sequence name="left_grasp_recovery_root">
<GripperCmd1_H name="s15_left_hand_gripper_open" />
<Arm_H name="s15_left_arm_pre_grasp" />
<Arm_H name="s15_left_arm_pre_cam_take_photo" />
<Arm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="arm_vision_recovery_subtree">
<Sequence name="arm_vision_recovery_root">
<Parallel name="arm_initial3">
<Arm_H name="s16_right_arm_initial" />
<Arm_H name="s16_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_arm_vision" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="left_arm_vision_recovery_subtree">
<Sequence name="left_arm_vision_recovery_root">
<Parallel name="left_arm_initial">
<Arm_H name="s17_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" />
</Sequence>
</BehaviorTree>
<BehaviorTree ID="right_arm_vision_recovery_subtree">
<Sequence name="right_arm_vision_recovery_root">
<Parallel name="right_arm_initial">
<Arm_H name="s17_right_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
</Sequence>
</BehaviorTree>
</root>

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

@@ -43,20 +43,29 @@ public:
*/
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
std::vector<geometry_msgs::msg::Pose> right_hand_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> grasp_poses_;
std::vector<geometry_msgs::msg::Pose> right_hand_take_object_pose_;
std::vector<geometry_msgs::msg::Pose> left_arm_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> left_arm_pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> left_arm_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> left_arm_take_object_pose_;
std::vector<std::vector<double>> right_hand_take_photo_angles_;
std::vector<std::vector<double>> pre_grasp_angles_;
std::vector<std::vector<double>> grasp_angles_;
std::vector<std::vector<double>> right_hand_take_object_angles_;
std::vector<geometry_msgs::msg::Pose> right_arm_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> right_arm_pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> right_arm_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> right_arm_take_object_pose_;
std::vector<std::vector<double>> left_arm_take_photo_angles_;
std::vector<std::vector<double>> left_arm_pre_grasp_angles_;
std::vector<std::vector<double>> left_arm_grasp_angles_;
std::vector<std::vector<double>> left_arm_take_object_angles_;
std::vector<std::vector<double>> right_arm_take_photo_angles_;
std::vector<std::vector<double>> right_arm_pre_grasp_angles_;
std::vector<std::vector<double>> right_arm_grasp_angles_;
std::vector<std::vector<double>> right_arm_take_object_angles_;
std::vector<double> left_arm_joint_angles_;
std::vector<double> right_arm_joint_angles_;
std::mutex joint_state_mutex_;
std::vector<double> right_arm_take_photo_joint_angles_;
std::string camera_position_;
@@ -125,7 +134,7 @@ private:
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
void JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
std::vector<double> grab_width_;
std::vector<double> right_hand_grab_width_;
// Current accepted sequence epoch (from client) used to filter/reject stale goals
std::atomic<uint32_t> current_epoch_{0};
@@ -423,7 +432,7 @@ private:
/** @brief 将检测到的姿态转换到机械臂参考坐标系。 */
bool TransformPoseToArmFrame(
const geometry_msgs::msg::PoseStamped & source,
const geometry_msgs::msg::PoseStamped & source, const std::string & arm_target_frame,
geometry_msgs::msg::PoseStamped & transformed) const;
// NOTE: Additional future helpers (cancellation handling, concurrency) may be added here.

298
src/brain/src/cerebellum_hooks.cpp
View File

@@ -83,32 +83,68 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
geometry_msgs::msg::Pose pose;
std::vector<double> angle{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
if (goal.data_type == 100) { //take photo
if (!node->right_hand_take_photo_poses_.empty()) {
pose = node->right_hand_take_photo_poses_[0];
}
if (!node->right_hand_take_photo_angles_.empty()) {
angle = node->right_hand_take_photo_angles_[node->right_hand_take_photo_angles_.size()-1];
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_photo_poses_.empty()) {
pose = node->right_arm_take_photo_poses_[0];
}
if (!node->right_arm_take_photo_angles_.empty()) {
angle = node->right_arm_take_photo_angles_[node->right_arm_take_photo_angles_.size()-1];
}
} else {
if (!node->left_arm_take_photo_poses_.empty()) {
pose = node->left_arm_take_photo_poses_[0];
}
if (!node->left_arm_take_photo_angles_.empty()) {
angle = node->left_arm_take_photo_angles_[node->left_arm_take_photo_angles_.size()-1];
}
}
} else if (goal.data_type == 101) { // pre-grasp
if (!node->pre_grasp_poses_.empty()) {
pose = node->pre_grasp_poses_[0];
}
if (!node->pre_grasp_angles_.empty()) {
angle = node->pre_grasp_angles_[node->pre_grasp_angles_.size()-1];
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_pre_grasp_poses_.empty()) {
pose = node->right_arm_pre_grasp_poses_[0];
}
if (!node->right_arm_pre_grasp_angles_.empty()) {
angle = node->right_arm_pre_grasp_angles_[node->right_arm_pre_grasp_angles_.size()-1];
}
} else {
if (!node->left_arm_pre_grasp_poses_.empty()) {
pose = node->left_arm_pre_grasp_poses_[0];
}
if (!node->left_arm_pre_grasp_angles_.empty()) {
angle = node->left_arm_pre_grasp_angles_[node->left_arm_pre_grasp_angles_.size()-1];
}
}
} else if (goal.data_type == 102) { // grasp
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[0];
}
if (!node->grasp_angles_.empty()) {
angle = node->grasp_angles_[node->grasp_angles_.size()-1];
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_grasp_poses_.empty()) {
pose = node->right_arm_grasp_poses_[0];
}
if (!node->right_arm_grasp_angles_.empty()) {
angle = node->right_arm_grasp_angles_[node->right_arm_grasp_angles_.size()-1];
}
} else {
if (!node->left_arm_grasp_poses_.empty()) {
pose = node->left_arm_grasp_poses_[0];
}
if (!node->left_arm_grasp_angles_.empty()) {
angle = node->left_arm_grasp_angles_[node->left_arm_grasp_angles_.size()-1];
}
}
} else if (goal.data_type == 110) { //take object
if (!node->right_hand_take_object_pose_.empty()) {
pose = node->right_hand_take_object_pose_[0];
}
if (!node->right_hand_take_object_angles_.empty()) {
angle = node->right_hand_take_object_angles_[node->right_hand_take_object_angles_.size()-1];
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_object_pose_.empty()) {
pose = node->right_arm_take_object_pose_[0];
}
if (!node->right_arm_take_object_angles_.empty()) {
angle = node->right_arm_take_object_angles_[node->right_arm_take_object_angles_.size()-1];
}
} else {
if (!node->left_arm_take_object_pose_.empty()) {
pose = node->left_arm_take_object_pose_[0];
}
if (!node->left_arm_take_object_angles_.empty()) {
angle = node->left_arm_take_object_angles_[node->left_arm_take_object_angles_.size()-1];
}
}
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
@@ -514,7 +550,7 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
return goal;
}
// goal.loc = 30; //grab_width_; //TODO transfer
// goal.loc = 30; //right_hand_grab_width_; //TODO transfer
RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, loc: %d, speed: %d, torque: %d, mode: %d",
skill_name.c_str(), goal.loc, goal.speed, goal.torque, goal.mode);
@@ -585,17 +621,21 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
for (auto &tf: node->target_frames_) {
if (tf == obj.class_name) {
node->target_pose_[tf].header = header;
node->target_pose_[tf].header.frame_id = "RightLink6";
if (node->camera_position_ == "left") {
node->target_pose_[tf].header.frame_id = "LeftLink6";
} else if (node->camera_position_ == "right") {
node->target_pose_[tf].header.frame_id = "RightLink6";
}
node->target_pose_[tf].pose = obj.pose;
node->grab_width_.clear();
node->right_hand_grab_width_.clear();
int size = (int)(obj.grab_width.size());
for (int i = 0; i < size; i++) {
node->grab_width_.push_back(obj.grab_width[i]);
node->right_hand_grab_width_.push_back(obj.grab_width[i]);
RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", obj.grab_width[i]);
}
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->grab_width_.size(),
RCLCPP_INFO(node->get_logger(), "target_frame: %s, right_hand_grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->right_hand_grab_width_.size(),
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
} else {
@@ -607,39 +647,45 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
};
auto CalculateGraspPoses = [node](const std::string & skill_name) -> bool {
// const auto & p = node->target_pose_[node->target_frame_].pose;
// RCLCPP_INFO(node->get_logger(), "target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
// p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if ((node->arm_target_frame_.find("right") != std::string::npos)) {
current_joint_angles = node->right_arm_joint_angles_;
} else {
current_joint_angles = node->left_arm_joint_angles_;
}
}
//top cam
if (node->camera_position_ == "top") {
node->right_hand_take_photo_poses_.clear();
node->right_hand_take_photo_angles_.clear();
std::string target_arm = "";
geometry_msgs::msg::Pose pose = node->target_pose_[node->target_frame_].pose;
RCLCPP_INFO(node->get_logger(), "[%s] target pose %s frame, camera_position: %s",
skill_name.c_str(), node->arm_target_frame_.c_str(), node->camera_position_.c_str());
RCLCPP_INFO(node->get_logger(), "target pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
geometry_msgs::msg::PoseStamped target_pose = node->target_pose_[node->target_frame_];
target_pose.header.frame_id = "base_link_rightarm";
pose.position.x += 0.08f;
// pose.position.y -= 0.40f;
pose.position.y = 0.30f;
pose.position.z += 0.03f;
pose.orientation.x = -0.7071;
pose.orientation.y = 0.0000;
pose.orientation.z = -0.0000;
pose.orientation.w = 0.7071;
if (target_pose.pose.position.z > 0.10f) { //right arm
target_arm = "right_arm";
target_pose.pose.position.x += 0.08f;
target_pose.pose.position.y = 0.30f;
target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = -0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
target_pose.pose.orientation.w = 0.7071;
} else if (target_pose.pose.position.z < -0.15f) { //left arm
//transfer to left arm base
target_arm = "left_arm";
geometry_msgs::msg::PoseStamped left_arm_pose;
const bool transformed = node->TransformPoseToArmFrame(target_pose, "base_link_leftarm", left_arm_pose);
if (!transformed) {
RCLCPP_WARN(node->get_logger(), "[%s] coordinate transformation failed, continuing with %s frame data",
skill_name.c_str(), left_arm_pose.header.frame_id.c_str());
return false;
}
target_pose.pose = left_arm_pose.pose;
target_pose.pose.position.x += 0.08f;
target_pose.pose.position.y = 0.30f;
target_pose.pose.position.z += 0.03f;
target_pose.pose.orientation.x = 0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
target_pose.pose.orientation.w = 0.7071;
}
auto pose = target_pose.pose;
RCLCPP_INFO(node->get_logger(), "cam take photo pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
@@ -647,11 +693,20 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.1 ||
pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
RCLCPP_ERROR(node->get_logger(), "invalid right_arm pose value, cal grasp pose failed");
return false;
}
std::vector<double> joint_angles = current_joint_angles;
std::vector<double> joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if(target_arm == "right_arm") {
joint_angles = node->right_arm_joint_angles_;
} else if (target_arm == "left_arm") {
joint_angles = node->left_arm_joint_angles_;
}
}
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(target_pos, target_quat, joint_angles) != 0) {
@@ -666,14 +721,32 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
RCLCPP_ERROR(node->get_logger(), "right_hand take_photo joint angles size invalid: %ld", joint_angles.size());
}
#endif
node->right_hand_take_photo_poses_.push_back(pose);
node->right_hand_take_photo_angles_.push_back(joint_angles);
if(target_arm == "right_arm") {
node->right_arm_take_photo_poses_.push_back(pose);
node->right_arm_take_photo_angles_.push_back(joint_angles);
} else if (target_arm == "left_arm") {
node->left_arm_take_photo_poses_.push_back(pose);
node->left_arm_take_photo_angles_.push_back(joint_angles);
}
return true;
}
//left & right cam
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if (node->camera_position_ == "right") {
current_joint_angles = node->right_arm_joint_angles_;
} else if (node->camera_position_ == "left") {
current_joint_angles = node->left_arm_joint_angles_;
}
}
geometry_msgs::msg::PoseStamped pose_in_arm;
const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], node->arm_target_frame_, pose_in_arm);
if (!transformed) {
RCLCPP_WARN(node->get_logger(), "[%s] coordinate transformation failed, continuing with %s frame data",
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
@@ -794,13 +867,21 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
take_object_msg.orientation.z = take_object_pose.orientation.z();
take_object_msg.orientation.w = take_object_pose.orientation.w();
node->pre_grasp_poses_.push_back(pre_grasp_msg);
node->grasp_poses_.push_back(grasp_msg);
node->right_hand_take_object_pose_.push_back(take_object_msg);
node->pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->grasp_angles_.push_back(grasp_joint_angles);
node->right_hand_take_object_angles_.push_back(take_object_joint_angles);
if (node->camera_position_ == "right") {
node->right_arm_pre_grasp_poses_.push_back(pre_grasp_msg);
node->right_arm_grasp_poses_.push_back(grasp_msg);
node->right_arm_take_object_pose_.push_back(take_object_msg);
node->right_arm_pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->right_arm_grasp_angles_.push_back(grasp_joint_angles);
node->right_arm_take_object_angles_.push_back(take_object_joint_angles);
} else if (node->camera_position_ == "left") {
node->left_arm_pre_grasp_poses_.push_back(pre_grasp_msg);
node->left_arm_grasp_poses_.push_back(grasp_msg);
node->left_arm_take_object_pose_.push_back(take_object_msg);
node->left_arm_pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->left_arm_grasp_angles_.push_back(grasp_joint_angles);
node->left_arm_take_object_angles_.push_back(take_object_joint_angles);
}
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
}
@@ -831,9 +912,9 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// node->declare_parameter<std::string>(param, std::string("right"));
// }
// req->camera_position = node->get_parameter(param).as_string();
node->camera_position_ = req->camera_position;
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
return req;
};
hooks.on_response = [node, UpdateTargetPose, CalculateGraspPoses](
@@ -851,15 +932,28 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
skill_name.c_str(), resp->success, resp->objects.size(),
resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
node->pre_grasp_poses_.clear();
node->grasp_poses_.clear();
node->right_hand_take_object_pose_.clear();
node->pre_grasp_angles_.clear();
node->grasp_angles_.clear();
node->right_hand_take_object_angles_.clear();
skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
if (node->camera_position_ == "left") {
node->left_arm_pre_grasp_poses_.clear();
node->left_arm_grasp_poses_.clear();
node->left_arm_take_object_pose_.clear();
node->left_arm_pre_grasp_angles_.clear();
node->left_arm_grasp_angles_.clear();
node->left_arm_take_object_angles_.clear();
} else if (node->camera_position_ == "right") {
node->right_arm_pre_grasp_poses_.clear();
node->right_arm_grasp_poses_.clear();
node->right_arm_take_object_pose_.clear();
node->right_arm_pre_grasp_angles_.clear();
node->right_arm_grasp_angles_.clear();
node->right_arm_take_object_angles_.clear();
} else if (node->camera_position_ == "top") {
node->left_arm_take_photo_poses_.clear();
node->left_arm_take_photo_angles_.clear();
node->right_arm_take_photo_poses_.clear();
node->right_arm_take_photo_angles_.clear();
}
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
@@ -875,41 +969,61 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
if (node->camera_position_ == "top") {
if (node->right_hand_take_photo_poses_.empty()) {
if (node->right_arm_take_photo_poses_.empty() || node->left_arm_take_photo_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->right_hand_take_photo_angles_.empty()) {
if (node->right_arm_take_photo_angles_.empty() || node->left_arm_take_photo_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo angles generated", skill_name.c_str());
return false;
}
} else {
if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
} else if (node->camera_position_ == "right") {
if (node->right_arm_pre_grasp_poses_.empty() || node->right_arm_grasp_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->pre_grasp_poses_.size() != node->grasp_poses_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] pre_grasp_poses_ size %zu not match grasp_poses_ size %zu",
skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
if (node->right_arm_pre_grasp_poses_.size() != node->right_arm_grasp_poses_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] right_arm_pre_grasp_poses_ size %zu not match right_arm_grasp_poses_ size %zu",
skill_name.c_str(), node->right_arm_pre_grasp_poses_.size(), node->right_arm_grasp_poses_.size());
return false;
}
if (node->pre_grasp_angles_.empty() || node->grasp_angles_.empty() || node->right_hand_take_object_angles_.empty()) {
if (node->right_arm_pre_grasp_angles_.empty() || node->right_arm_grasp_angles_.empty() || node->right_arm_take_object_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->pre_grasp_angles_.size() != node->grasp_angles_.size() ||
node->grasp_angles_.size() != node->right_hand_take_object_angles_.size() ||
node->pre_grasp_angles_.size() != node->right_hand_take_object_angles_.size()) {
if (node->right_arm_pre_grasp_angles_.size() != node->right_arm_grasp_angles_.size() ||
node->right_arm_grasp_angles_.size() != node->right_arm_take_object_angles_.size() ||
node->right_arm_pre_grasp_angles_.size() != node->right_arm_take_object_angles_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->pre_grasp_angles_.size(), node->grasp_angles_.size(),
node->right_hand_take_object_angles_.size());
skill_name.c_str(), node->right_arm_pre_grasp_angles_.size(), node->right_arm_grasp_angles_.size(), node->right_arm_take_object_angles_.size());
return false;
}
} else if (node->camera_position_ == "left") {
if (node->left_arm_pre_grasp_poses_.empty() || node->left_arm_grasp_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->left_arm_pre_grasp_poses_.size() != node->left_arm_grasp_poses_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] left_arm_pre_grasp_poses_ size %zu not match left_arm_grasp_poses_ size %zu",
skill_name.c_str(), node->left_arm_pre_grasp_poses_.size(), node->left_arm_grasp_poses_.size());
return false;
}
if (node->left_arm_pre_grasp_angles_.empty() || node->left_arm_grasp_angles_.empty() || node->left_arm_take_object_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->left_arm_pre_grasp_angles_.size() != node->left_arm_grasp_angles_.size() ||
node->left_arm_grasp_angles_.size() != node->left_arm_take_object_angles_.size() ||
node->left_arm_pre_grasp_angles_.size() != node->left_arm_take_object_angles_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->left_arm_pre_grasp_angles_.size(), node->left_arm_grasp_angles_.size(), node->left_arm_take_object_angles_.size());
return false;
}
}
node->right_arm_take_photo_joint_angles_.clear();
node->right_arm_take_photo_joint_angles_ = node->right_arm_joint_angles_;
// node->camera_position_.clear();
return true;
};

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

@@ -324,7 +324,7 @@ void CerebellumNode::SetupExecuteBtServer()
}
bool CerebellumNode::TransformPoseToArmFrame(
const geometry_msgs::msg::PoseStamped & source,
const geometry_msgs::msg::PoseStamped & source, const std::string & arm_target_frame,
geometry_msgs::msg::PoseStamped & transformed) const
{
if (!tf_buffer_) {
@@ -335,14 +335,14 @@ bool CerebellumNode::TransformPoseToArmFrame(
RCLCPP_WARN(this->get_logger(), "Target pose missing source frame, skipping transformation");
return false;
}
if (source.header.frame_id == arm_target_frame_) {
RCLCPP_INFO(this->get_logger(), "Target pose already in %s frame, skipping transformation", arm_target_frame_.c_str());
if (source.header.frame_id == arm_target_frame) {
RCLCPP_INFO(this->get_logger(), "Target pose already in %s frame, skipping transformation", arm_target_frame.c_str());
transformed = source;
return true;
}
try {
transformed = tf_buffer_->transform(
source, arm_target_frame_, tf2::durationFromSec(arm_transform_timeout_sec_));
source, arm_target_frame, tf2::durationFromSec(arm_transform_timeout_sec_));
// ===================
#ifdef TF_DEBUG_LOGGING
@@ -363,12 +363,12 @@ bool CerebellumNode::TransformPoseToArmFrame(
RCLCPP_INFO(
this->get_logger(), "Target pose transformed from %s to %s",
source.header.frame_id.c_str(), arm_target_frame_.c_str());
source.header.frame_id.c_str(), arm_target_frame.c_str());
return true;
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(
this->get_logger(), "Failed to transform target pose: %s -> %s, reason: %s",
source.header.frame_id.c_str(), arm_target_frame_.c_str(), ex.what());
source.header.frame_id.c_str(), arm_target_frame.c_str(), ex.what());
}
return false;
}