Skip to content

Commit 1f83f3a

Browse files
motty-mio2motty-mio2
committed
表現を統一
1 parent 0993add commit 1f83f3a

4 files changed

Lines changed: 232 additions & 156 deletions

File tree

‎crane_x7_examples/src/pick_and_place.cpp‎

Lines changed: 48 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,11 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;
3131
class PickAndPlace
3232
{
3333
public:
34+
// グリッパの開閉角度
35+
inline static const double GRIPPER_OPEN = angles::from_degrees(60.0);
36+
inline static const double GRIPPER_GRASP = angles::from_degrees(20.0);
37+
inline static const double GRIPPER_CLOSE = 0.0;
38+
3439
// ノードを受け取り、アーム・グリッパのMoveGroupInterfaceを初期化する
3540
explicit PickAndPlace(rclcpp::Node::SharedPtr node)
3641
{
@@ -48,6 +53,21 @@ class PickAndPlace
4853
move_group_arm_->move();
4954
}
5055

56+
// アームを目標位置（x, y, z [m]）・姿勢（roll, pitch, yaw [deg]）に動かす
57+
void control_arm(
58+
const double x, const double y, const double z,
59+
const double roll, const double pitch, const double yaw)
60+
{
61+
geometry_msgs::msg::Pose target_pose;
62+
tf2::Quaternion q;
63+
target_pose.position.x = x;
64+
target_pose.position.y = y;
65+
target_pose.position.z = z;
66+
q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw));
67+
target_pose.orientation = tf2::toMsg(q);
68+
move_arm_to_pose(target_pose);
69+
}
70+
5171
// SRDFに定義された姿勢名でアームを動かす
5272
void move_arm_to_named_pose(const std::string & name)
5373
{
@@ -64,14 +84,32 @@ class PickAndPlace
6484
move_group_gripper_->move();
6585
}
6686

67-
// アームの可動範囲に制約を設定する
68-
void set_arm_path_constraints(const moveit_msgs::msg::Constraints & constraints)
87+
// アームの関節の一部に可動制限を設定する
88+
void set_constraints()
6989
{
90+
moveit_msgs::msg::Constraints constraints;
91+
constraints.name = "arm_constraints";
92+
93+
moveit_msgs::msg::JointConstraint joint_constraint;
94+
joint_constraint.joint_name = "crane_x7_lower_arm_fixed_part_joint";
95+
joint_constraint.position = 0.0;
96+
joint_constraint.tolerance_above = angles::from_degrees(30);
97+
joint_constraint.tolerance_below = angles::from_degrees(30);
98+
joint_constraint.weight = 1.0;
99+
constraints.joint_constraints.push_back(joint_constraint);
100+
101+
joint_constraint.joint_name = "crane_x7_upper_arm_revolute_part_twist_joint";
102+
joint_constraint.position = 0.0;
103+
joint_constraint.tolerance_above = angles::from_degrees(30);
104+
joint_constraint.tolerance_below = angles::from_degrees(30);
105+
joint_constraint.weight = 0.8;
106+
constraints.joint_constraints.push_back(joint_constraint);
107+
70108
move_group_arm_->setPathConstraints(constraints);
71109
}
72110

73-
// アームの可動範囲の制約を解除する
74-
void clear_arm_path_constraints()
111+
// 設定された関節可動制限をクリアする
112+
void clear_constraints()
75113
{
76114
move_group_arm_->clearPathConstraints();
77115
}
@@ -93,11 +131,6 @@ int main(int argc, char ** argv)
93131

94132
PickAndPlace controller(node);
95133

96-
// グリッパの開閉角度
97-
const double GRIPPER_OPEN = angles::from_degrees(60.0);
98-
const double GRIPPER_GRASP = angles::from_degrees(20.0);
99-
const double GRIPPER_CLOSE = 0.0;
100-
101134
// アームの目標姿勢（hand down姿勢: RPY = -180, 0, -90 [deg]）
102135
tf2::Quaternion q;
103136
q.setRPY(angles::from_degrees(-180), angles::from_degrees(0), angles::from_degrees(-90));
@@ -119,46 +152,27 @@ int main(int argc, char ** argv)
119152
geometry_msgs::msg::Pose post_release_pose = pre_release_pose;
120153
post_release_pose.position.z = 0.2;
121154

122-
// アームの可動範囲制約
123-
moveit_msgs::msg::Constraints constraints;
124-
constraints.name = "arm_constraints";
125-
126-
moveit_msgs::msg::JointConstraint joint_constraint;
127-
joint_constraint.joint_name = "crane_x7_lower_arm_fixed_part_joint";
128-
joint_constraint.position = 0.0;
129-
joint_constraint.tolerance_above = angles::from_degrees(30);
130-
joint_constraint.tolerance_below = angles::from_degrees(30);
131-
joint_constraint.weight = 1.0;
132-
constraints.joint_constraints.push_back(joint_constraint);
133-
134-
joint_constraint.joint_name = "crane_x7_upper_arm_revolute_part_twist_joint";
135-
joint_constraint.position = 0.0;
136-
joint_constraint.tolerance_above = angles::from_degrees(30);
137-
joint_constraint.tolerance_below = angles::from_degrees(30);
138-
joint_constraint.weight = 0.8;
139-
constraints.joint_constraints.push_back(joint_constraint);
140-
141155
// 初期化動作
142156
controller.move_arm_to_named_pose("home");
143-
controller.set_gripper_angle(GRIPPER_OPEN); // 何かを掴んでいた時のために開く
144-
controller.set_arm_path_constraints(constraints);
157+
controller.set_gripper_angle(PickAndPlace::GRIPPER_OPEN); // 何かを掴んでいた時のために開く
158+
controller.set_constraints();
145159

146160
// ピック動作（掴みに行く）
147161
controller.move_arm_to_pose(pre_grasp_pose); // 物体の上に腕を伸ばす
148162
controller.move_arm_to_pose(grasp_pose); // アプローチ
149-
controller.set_gripper_angle(GRIPPER_GRASP); // 掴む
163+
controller.set_gripper_angle(PickAndPlace::GRIPPER_GRASP); // 掴む
150164
controller.move_arm_to_pose(pre_grasp_pose); // 持ち上げる
151165

152166
// プレース動作（移動して置く）
153167
controller.move_arm_to_pose(pre_release_pose); // 移動する
154168
controller.move_arm_to_pose(release_pose); // 下ろす
155-
controller.set_gripper_angle(GRIPPER_OPEN); // 離す
169+
controller.set_gripper_angle(PickAndPlace::GRIPPER_OPEN); // 離す
156170
controller.move_arm_to_pose(post_release_pose); // 少し持ち上げる
157171

158172
// 終了動作
159-
controller.clear_arm_path_constraints();
173+
controller.clear_constraints();
160174
controller.move_arm_to_named_pose("home");
161-
controller.set_gripper_angle(GRIPPER_CLOSE);
175+
controller.set_gripper_angle(PickAndPlace::GRIPPER_CLOSE);
162176

163177
rclcpp::shutdown();
164178
spin_thread.join();

‎crane_x7_examples/src/pick_and_place_tf.cpp‎

Lines changed: 57 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,11 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;
4040
class PickAndPlaceTf : public rclcpp::Node
4141
{
4242
public:
43+
// グリッパの開閉角度
44+
inline static const double GRIPPER_OPEN = angles::from_degrees(60.0);
45+
inline static const double GRIPPER_GRASP = angles::from_degrees(20.0);
46+
inline static const double GRIPPER_CLOSE = 0.0;
47+
4348
PickAndPlaceTf(
4449
rclcpp::Node::SharedPtr move_group_arm_node,
4550
rclcpp::Node::SharedPtr move_group_gripper_node)
@@ -52,29 +57,7 @@ class PickAndPlaceTf : public rclcpp::Node
5257
move_group_gripper_ = std::make_shared<MoveGroupInterface>(move_group_gripper_node, "gripper");
5358

5459
// SRDFに定義されている"home"の姿勢にする
55-
move_group_arm_->setNamedTarget("home");
56-
move_group_arm_->move();
57-
58-
// 可動範囲を制限する
59-
moveit_msgs::msg::Constraints constraints;
60-
constraints.name = "arm_constraints";
61-
62-
moveit_msgs::msg::JointConstraint joint_constraint;
63-
joint_constraint.joint_name = "crane_x7_lower_arm_fixed_part_joint";
64-
joint_constraint.position = 0.0;
65-
joint_constraint.tolerance_above = angles::from_degrees(30);
66-
joint_constraint.tolerance_below = angles::from_degrees(30);
67-
joint_constraint.weight = 1.0;
68-
constraints.joint_constraints.push_back(joint_constraint);
69-
70-
joint_constraint.joint_name = "crane_x7_upper_arm_revolute_part_twist_joint";
71-
joint_constraint.position = 0.0;
72-
joint_constraint.tolerance_above = angles::from_degrees(30);
73-
joint_constraint.tolerance_below = angles::from_degrees(30);
74-
joint_constraint.weight = 0.8;
75-
constraints.joint_constraints.push_back(joint_constraint);
76-
77-
move_group_arm_->setPathConstraints(constraints);
60+
move_arm_to_named_pose("home");
7861

7962
// 待機姿勢に移動する
8063
init_pose();
@@ -94,6 +77,13 @@ class PickAndPlaceTf : public rclcpp::Node
9477
move_group_gripper_->move();
9578
}
9679

80+
// アームを目標位置・姿勢（Pose）に動かす
81+
void move_arm_to_pose(const geometry_msgs::msg::Pose & pose)
82+
{
83+
move_group_arm_->setPoseTarget(pose);
84+
move_group_arm_->move();
85+
}
86+
9787
// アームを目標位置（x, y, z [m]）・姿勢（roll, pitch, yaw [deg]）に動かす
9888
void control_arm(
9989
const double x, const double y, const double z,
@@ -106,10 +96,46 @@ class PickAndPlaceTf : public rclcpp::Node
10696
target_pose.position.z = z;
10797
q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw));
10898
target_pose.orientation = tf2::toMsg(q);
109-
move_group_arm_->setPoseTarget(target_pose);
99+
move_arm_to_pose(target_pose);
100+
}
101+
102+
// SRDFに定義された姿勢名でアームを動かす
103+
void move_arm_to_named_pose(const std::string & name)
104+
{
105+
move_group_arm_->setNamedTarget(name);
110106
move_group_arm_->move();
111107
}
112108

109+
// アームの関節の一部に可動制限を設定する
110+
void set_constraints()
111+
{
112+
moveit_msgs::msg::Constraints constraints;
113+
constraints.name = "arm_constraints";
114+
115+
moveit_msgs::msg::JointConstraint joint_constraint;
116+
joint_constraint.joint_name = "crane_x7_lower_arm_fixed_part_joint";
117+
joint_constraint.position = 0.0;
118+
joint_constraint.tolerance_above = angles::from_degrees(30);
119+
joint_constraint.tolerance_below = angles::from_degrees(30);
120+
joint_constraint.weight = 1.0;
121+
constraints.joint_constraints.push_back(joint_constraint);
122+
123+
joint_constraint.joint_name = "crane_x7_upper_arm_revolute_part_twist_joint";
124+
joint_constraint.position = 0.0;
125+
joint_constraint.tolerance_above = angles::from_degrees(30);
126+
joint_constraint.tolerance_below = angles::from_degrees(30);
127+
joint_constraint.weight = 0.8;
128+
constraints.joint_constraints.push_back(joint_constraint);
129+
130+
move_group_arm_->setPathConstraints(constraints);
131+
}
132+
133+
// 設定された関節可動制限をクリアする
134+
void clear_constraints()
135+
{
136+
move_group_arm_->clearPathConstraints();
137+
}
138+
113139
private:
114140
void on_timer()
115141
{
@@ -179,19 +205,15 @@ class PickAndPlaceTf : public rclcpp::Node
179205

180206
void picking(tf2::Vector3 target_position)
181207
{
182-
const double GRIPPER_DEFAULT = 0.0;
183-
const double GRIPPER_OPEN = angles::from_degrees(60.0);
184-
const double GRIPPER_CLOSE = angles::from_degrees(20.0);
185-
186208
// 何かを掴んでいた時のためにハンドを開閉
187209
set_gripper_angle(GRIPPER_OPEN);
188-
set_gripper_angle(GRIPPER_DEFAULT);
210+
set_gripper_angle(GRIPPER_CLOSE);
189211

190212
// ピック動作（掴みに行く）
191213
control_arm(target_position.x(), target_position.y(), target_position.z() + 0.12, -180, 0, 90);
192214
set_gripper_angle(GRIPPER_OPEN);
193215
control_arm(target_position.x(), target_position.y(), target_position.z() + 0.07, -180, 0, 90);
194-
set_gripper_angle(GRIPPER_CLOSE);
216+
set_gripper_angle(GRIPPER_GRASP);
195217
control_arm(target_position.x(), target_position.y(), target_position.z() + 0.12, -180, 0, 90);
196218

197219
// プレース動作（移動して置く）
@@ -202,7 +224,7 @@ class PickAndPlaceTf : public rclcpp::Node
202224

203225
// 待機姿勢に戻る
204226
init_pose();
205-
set_gripper_angle(GRIPPER_DEFAULT);
227+
set_gripper_angle(GRIPPER_CLOSE);
206228
}
207229

208230
std::shared_ptr<MoveGroupInterface> move_group_arm_;
@@ -226,10 +248,14 @@ int main(int argc, char ** argv)
226248
auto pick_and_place_tf_node = std::make_shared<PickAndPlaceTf>(
227249
move_group_arm_node,
228250
move_group_gripper_node);
251+
pick_and_place_tf_node->set_constraints();
252+
229253
exec.add_node(pick_and_place_tf_node);
230254
exec.add_node(move_group_arm_node);
231255
exec.add_node(move_group_gripper_node);
232256
exec.spin();
257+
258+
pick_and_place_tf_node->clear_constraints();
233259
rclcpp::shutdown();
234260
return 0;
235261
}

0 commit comments

Comments
 (0)