From 572eefdd1253ae03d5e2caab561e0ccba98c685b Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Thu, 9 Nov 2023 16:03:15 +0900 Subject: [PATCH] =?UTF-8?q?sciurus17=5Fexamples=E3=81=AEROS=202=E5=AF=BE?= =?UTF-8?q?=E5=BF=9C=20(#144)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * allowed_start_tolerance設定 * gripper_controlサンプル追加 * neck_group, waist_group追加 * neck_controlサンプル追加 * waist_controlサンプル追加 * スタイル修正 * Gazebo上のcube位置変更 * pick_and_place_right_arm_waistサンプル追加 * 手先位置姿勢生成ライブラリ追加 * スタイル修正 * ポーズプリセットの追加 * ライブラリの更新に対応 * pick_and_place_left_armサンプル追加 * pick_and_place_two_armサンプル追加 * Revert "pick_and_place_two_armサンプル追加" This reverts commit 51522f97502cb10de0856d833e4e6efe0b2adf7b. * 不要なclassの削除 * 把持位置の変数化 * 変数名変更 * 変数名変更 * コメント修正 * コメント追加 * 駆動速度調整 * コメント追加 * 引数の定数化 * 不要なライブラリのincludeを削除 * シミュレータ動作が思いためmax_step_sizeを調整 * 把持位置調整 * ヘッダファイルのインクルード箇所を変更 * コメント修正 * グリッパ開閉角の定数化 --- sciurus17_examples/CMakeLists.txt | 45 ++++++ sciurus17_examples/include/pose_presets.hpp | 32 ++++ sciurus17_examples/launch/example.launch.py | 84 ++++++++++ sciurus17_examples/src/gripper_control.cpp | 99 ++++++++++++ sciurus17_examples/src/neck_control.cpp | 82 ++++++++++ .../src/pick_and_place_left_arm.cpp | 134 +++++++++++++++ .../src/pick_and_place_right_arm_waist.cpp | 152 ++++++++++++++++++ sciurus17_examples/src/pose_presets.cpp | 56 +++++++ sciurus17_examples/src/waist_control.cpp | 67 ++++++++ sciurus17_gazebo/worlds/table.sdf | 6 +- .../config/joint_limits.yaml | 10 ++ .../config/moveit_controllers.yaml | 3 + sciurus17_moveit_config/config/sciurus17.srdf | 14 ++ 13 files changed, 781 insertions(+), 3 deletions(-) create mode 100644 sciurus17_examples/include/pose_presets.hpp create mode 100644 sciurus17_examples/launch/example.launch.py create mode 100644 sciurus17_examples/src/gripper_control.cpp create mode 100644 sciurus17_examples/src/neck_control.cpp create mode 100644 sciurus17_examples/src/pick_and_place_left_arm.cpp create mode 100644 sciurus17_examples/src/pick_and_place_right_arm_waist.cpp create mode 100644 sciurus17_examples/src/pose_presets.cpp create mode 100644 sciurus17_examples/src/waist_control.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index bebaa0fd..728ca6c0 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -17,16 +17,61 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +set(LIBRARY_NAME "pose_presets") +add_library(${LIBRARY_NAME} + SHARED + src/${LIBRARY_NAME}.cpp +) +target_include_directories( + ${LIBRARY_NAME} + PUBLIC + $ + $ +) +ament_target_dependencies( + ${LIBRARY_NAME} + angles + geometry_msgs + tf2_geometry_msgs +) + +ament_export_targets(export_${LIBRARY_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + angles + geometry_msgs + tf2_geometry_msgs +) +install( + DIRECTORY include/ + DESTINATION include/${LIBRARY_NAME} +) + +install( + TARGETS ${LIBRARY_NAME} + EXPORT export_${LIBRARY_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + # Build and install node executables set(executable_list + gripper_control + neck_control + waist_control + pick_and_place_right_arm_waist + pick_and_place_left_arm ) foreach(loop_var IN LISTS executable_list) add_executable(${loop_var} src/${loop_var}.cpp) + target_link_libraries(${loop_var} ${LIBRARY_NAME}) ament_target_dependencies(${loop_var} geometry_msgs moveit_ros_planning_interface diff --git a/sciurus17_examples/include/pose_presets.hpp b/sciurus17_examples/include/pose_presets.hpp new file mode 100644 index 00000000..dba4f8fe --- /dev/null +++ b/sciurus17_examples/include/pose_presets.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_PRESETS_HPP_ +#define POSE_PRESETS_HPP_ + +#include "geometry_msgs/msg/pose.hpp" + +namespace pose_presets +{ +// Pose型の位置姿勢を作成 +geometry_msgs::msg::Pose generate_pose( + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw); +// 右グリッパを下に向ける姿勢を作成 +geometry_msgs::msg::Pose right_arm_downward(const double x, const double y, const double z); +// 左グリッパを下に向ける姿勢を作成 +geometry_msgs::msg::Pose left_arm_downward(const double x, const double y, const double z); +} // namespace pose_presets + +#endif // POSE_PRESETS_HPP_ diff --git a/sciurus17_examples/launch/example.launch.py b/sciurus17_examples/launch/example.launch.py new file mode 100644 index 00000000..959ddc62 --- /dev/null +++ b/sciurus17_examples/launch/example.launch.py @@ -0,0 +1,84 @@ +# Copyright 2023 RT Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import SetParameter +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import yaml + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + description_loader = RobotDescriptionLoader() + + robot_description_semantic_config = load_file( + 'sciurus17_moveit_config', 'config/sciurus17.srdf') + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config} + + kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml') + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='gripper_control', + description=('Set an example executable name: ' + '[gripper_control, neck_control, waist_control,' + 'pick_and_place_right_arm_waist, pick_and_place_left_arm]') + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description=('Set true when using the gazebo simulator.') + ) + + example_node = Node(name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[{'robot_description': description_loader.load()}, + robot_description_semantic, + kinematics_yaml]) + + return LaunchDescription([ + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + declare_example_name, + example_node + ]) diff --git a/sciurus17_examples/src/gripper_control.cpp b/sciurus17_examples/src/gripper_control.cpp new file mode 100644 index 00000000..5a779376 --- /dev/null +++ b/sciurus17_examples/src/gripper_control.cpp @@ -0,0 +1,99 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +// examples/move_group_interface/src/move_group_interface_tutorial.cpp + +#include + +#include "angles/angles.h" +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_arm_node = + rclcpp::Node::make_shared("move_group_arm_node", node_options); + auto move_group_r_gripper_node = + rclcpp::Node::make_shared("move_group_r_gripper_node", node_options); + auto move_group_l_gripper_node = + rclcpp::Node::make_shared("move_group_l_gripper_node", node_options); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_arm_node); + executor.add_node(move_group_r_gripper_node); + executor.add_node(move_group_l_gripper_node); + std::thread([&executor]() {executor.spin();}).detach(); + + // 両腕制御用MoveGroupInterface + MoveGroupInterface move_group_arm(move_group_arm_node, "two_arm_group"); + // 駆動速度の調整 + move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_arm.setMaxAccelerationScalingFactor(0.1); // Set 0.0 ~ 1.0 + + // 右グリッパ制御用MoveGroupInterface + MoveGroupInterface move_group_r_gripper(move_group_r_gripper_node, "r_gripper_group"); + // 駆動速度の調整 + move_group_r_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_r_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + auto r_gripper_joint_values = move_group_r_gripper.getCurrentJointValues(); + const double R_GRIPPER_CLOSE = 0.0; + const double R_GRIPPER_OPEN = angles::from_degrees(40.0); + + // 左グリッパ制御用MoveGroupInterface + MoveGroupInterface move_group_l_gripper(move_group_l_gripper_node, "l_gripper_group"); + // 駆動速度の調整 + move_group_l_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_l_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + auto l_gripper_joint_values = move_group_l_gripper.getCurrentJointValues(); + const double L_GRIPPER_CLOSE = 0.0; + const double L_GRIPPER_OPEN = angles::from_degrees(-40.0); + + // SRDFに定義されている"two_arm_init_pose"の姿勢にする + move_group_arm.setNamedTarget("two_arm_init_pose"); + move_group_arm.move(); + + // 右グリッパ開閉 + for (int i = 0; i < 2; i++) { + r_gripper_joint_values[0] = R_GRIPPER_OPEN; + move_group_r_gripper.setJointValueTarget(r_gripper_joint_values); + move_group_r_gripper.move(); + + r_gripper_joint_values[0] = R_GRIPPER_CLOSE; + move_group_r_gripper.setJointValueTarget(r_gripper_joint_values); + move_group_r_gripper.move(); + } + + // 左グリッパ開閉 + for (int i = 0; i < 2; i++) { + l_gripper_joint_values[0] = L_GRIPPER_OPEN; + move_group_l_gripper.setJointValueTarget(l_gripper_joint_values); + move_group_l_gripper.move(); + + l_gripper_joint_values[0] = L_GRIPPER_CLOSE; + move_group_l_gripper.setJointValueTarget(l_gripper_joint_values); + move_group_l_gripper.move(); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_examples/src/neck_control.cpp b/sciurus17_examples/src/neck_control.cpp new file mode 100644 index 00000000..ee3e552e --- /dev/null +++ b/sciurus17_examples/src/neck_control.cpp @@ -0,0 +1,82 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +// examples/move_group_interface/src/move_group_interface_tutorial.cpp + +#include "angles/angles.h" +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("neck_control"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("neck_control", node_options); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() {executor.spin();}).detach(); + + // 首制御用MoveGroupInterface + MoveGroupInterface move_group_neck(move_group_node, "neck_group"); + // 駆動速度の調整 + move_group_neck.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_neck.setMaxAccelerationScalingFactor(0.1); // Set 0.0 ~ 1.0 + + // SRDFに定義されている"neck_init_pose"の姿勢にする + move_group_neck.setNamedTarget("neck_init_pose"); + move_group_neck.move(); + + // 現在角度をベースに、目標角度を作成する + auto joint_values = move_group_neck.getCurrentJointValues(); + + // 首を左に向ける + joint_values[0] = angles::from_degrees(45.0); + move_group_neck.setJointValueTarget(joint_values); + move_group_neck.move(); + + // 首を右に向ける + joint_values[0] = angles::from_degrees(-45.0); + move_group_neck.setJointValueTarget(joint_values); + move_group_neck.move(); + + // 首を前に向ける + joint_values[0] = angles::from_degrees(0.0); + move_group_neck.setJointValueTarget(joint_values); + move_group_neck.move(); + + // 首を上に向ける + joint_values[1] = angles::from_degrees(45.0); + move_group_neck.setJointValueTarget(joint_values); + move_group_neck.move(); + + // 首を下に向ける + joint_values[1] = angles::from_degrees(-45.0); + move_group_neck.setJointValueTarget(joint_values); + move_group_neck.move(); + + // "neck_init_pose"に戻す + move_group_neck.setNamedTarget("neck_init_pose"); + move_group_neck.move(); + + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_examples/src/pick_and_place_left_arm.cpp b/sciurus17_examples/src/pick_and_place_left_arm.cpp new file mode 100644 index 00000000..c5c95e83 --- /dev/null +++ b/sciurus17_examples/src/pick_and_place_left_arm.cpp @@ -0,0 +1,134 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +// examples/move_group_interface/src/move_group_interface_tutorial.cpp + +#include + +#include "angles/angles.h" +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" +#include "pose_presets.hpp" + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place_left_arm"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); + auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_arm_node); + executor.add_node(move_group_gripper_node); + std::thread([&executor]() {executor.spin();}).detach(); + + // 左腕制御用MoveGroupInterface + MoveGroupInterface move_group_arm(move_group_arm_node, "l_arm_group"); + // 駆動速度の調整 + move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_arm.setMaxAccelerationScalingFactor(0.1); // Set 0.0 ~ 1.0 + + // 左グリッパ制御用MoveGroupInterface + MoveGroupInterface move_group_gripper(move_group_gripper_node, "l_gripper_group"); + // 駆動速度の調整 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + + // グリッパの開閉角 + auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); + const double GRIPPER_CLOSE = 0.0; + const double GRIPPER_OPEN = angles::from_degrees(-40.0); + const double GRIPPER_GRASP = angles::from_degrees(-20.0); + + // 物体を掴む位置 + const double PICK_POSITION_X = 0.25; + const double PICK_POSITION_Y = 0.0; + const double PICK_POSITION_Z = 0.12; + + // 物体を置く位置 + const double PLACE_POSITION_X = 0.35; + const double PLACE_POSITION_Y = 0.0; + const double PLACE_POSITION_Z = 0.12; + + // 物体を持ち上げる高さ + const double LIFTING_HEIGHT = 0.25; + + // SRDFに定義されている"l_arm_init_pose"の姿勢にする + move_group_arm.setNamedTarget("l_arm_init_pose"); + move_group_arm.move(); + + // 何かを掴んでいた時のためにハンドを開く + gripper_joint_values[0] = GRIPPER_OPEN; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // 物体の上に腕を伸ばす + move_group_arm.setPoseTarget( + pose_presets::left_arm_downward(PICK_POSITION_X, PICK_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 掴みに行く + move_group_arm.setPoseTarget( + pose_presets::left_arm_downward(PICK_POSITION_X, PICK_POSITION_Y, PICK_POSITION_Z)); + move_group_arm.move(); + + // ハンドを閉じる + gripper_joint_values[0] = GRIPPER_GRASP; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // 持ち上げる + move_group_arm.setPoseTarget( + pose_presets::left_arm_downward(PICK_POSITION_X, PICK_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 移動する + move_group_arm.setPoseTarget( + pose_presets::left_arm_downward(PLACE_POSITION_X, PLACE_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 下ろす + move_group_arm.setPoseTarget( + pose_presets::left_arm_downward(PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z)); + move_group_arm.move(); + + // ハンドを開く + gripper_joint_values[0] = GRIPPER_OPEN; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // ハンドを持ち上げる + move_group_arm.setPoseTarget( + pose_presets::left_arm_downward(PLACE_POSITION_X, PLACE_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // SRDFに定義されている"l_arm_init_pose"の姿勢にする + move_group_arm.setNamedTarget("l_arm_init_pose"); + move_group_arm.move(); + + // ハンドを閉じる + gripper_joint_values[0] = GRIPPER_CLOSE; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_examples/src/pick_and_place_right_arm_waist.cpp b/sciurus17_examples/src/pick_and_place_right_arm_waist.cpp new file mode 100644 index 00000000..ab9d4004 --- /dev/null +++ b/sciurus17_examples/src/pick_and_place_right_arm_waist.cpp @@ -0,0 +1,152 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +// examples/move_group_interface/src/move_group_interface_tutorial.cpp + +#include + +#include "angles/angles.h" +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" +#include "pose_presets.hpp" + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place_right_arm_waist"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); + auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_arm_node); + executor.add_node(move_group_gripper_node); + std::thread([&executor]() {executor.spin();}).detach(); + + // 右腕と腰制御用MoveGroupInterface + MoveGroupInterface move_group_arm(move_group_arm_node, "r_arm_waist_group"); + // 駆動速度の調整 + move_group_arm.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_arm.setMaxAccelerationScalingFactor(0.1); // Set 0.0 ~ 1.0 + + // 右グリッパ制御用MoveGroupInterface + MoveGroupInterface move_group_gripper(move_group_gripper_node, "r_gripper_group"); + // 駆動速度の調整 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + + // グリッパの開閉角 + auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); + const double GRIPPER_CLOSE = 0.0; + const double GRIPPER_OPEN = angles::from_degrees(40.0); + const double GRIPPER_GRASP = angles::from_degrees(20.0); + + // 物体を掴む位置 + const double PICK_POSITION_X = 0.25; + const double PICK_POSITION_Y = 0.0; + const double PICK_POSITION_Z = 0.12; + + // 物体を置く位置 + const double PLACE_POSITION_X = 0.35; + const double PLACE_POSITION_Y = 0.0; + const double PLACE_POSITION_Z = 0.12; + + // 物体を持ち上げる高さ + const double LIFTING_HEIGHT = 0.25; + + // SRDFに定義されている"r_arm_waist_init_pose"の姿勢にする + move_group_arm.setNamedTarget("r_arm_waist_init_pose"); + move_group_arm.move(); + + // 何かを掴んでいた時のためにハンドを開く + gripper_joint_values[0] = GRIPPER_OPEN; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // 可動範囲を制限する + moveit_msgs::msg::Constraints constraints; + constraints.name = "arm_constraints"; + + // 腰軸の可動範囲を制限する + moveit_msgs::msg::JointConstraint joint_constraint; + joint_constraint.joint_name = "waist_yaw_joint"; + joint_constraint.position = 0.0; + joint_constraint.tolerance_above = angles::from_degrees(45); + joint_constraint.tolerance_below = angles::from_degrees(45); + joint_constraint.weight = 1.0; + constraints.joint_constraints.push_back(joint_constraint); + + move_group_arm.setPathConstraints(constraints); + + // 物体の上に腕を伸ばす + move_group_arm.setPoseTarget( + pose_presets::right_arm_downward(PICK_POSITION_X, PICK_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 掴みに行く + move_group_arm.setPoseTarget( + pose_presets::right_arm_downward(PICK_POSITION_X, PICK_POSITION_Y, PICK_POSITION_Z)); + move_group_arm.move(); + + // ハンドを閉じる + gripper_joint_values[0] = GRIPPER_GRASP; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // 持ち上げる + move_group_arm.setPoseTarget( + pose_presets::right_arm_downward(PICK_POSITION_X, PICK_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 移動する + move_group_arm.setPoseTarget( + pose_presets::right_arm_downward(PLACE_POSITION_X, PLACE_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 下ろす + move_group_arm.setPoseTarget( + pose_presets::right_arm_downward(PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z)); + move_group_arm.move(); + + // ハンドを開く + gripper_joint_values[0] = GRIPPER_OPEN; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + // 少しだけハンドを持ち上げる + move_group_arm.setPoseTarget( + pose_presets::right_arm_downward(PLACE_POSITION_X, PLACE_POSITION_Y, LIFTING_HEIGHT)); + move_group_arm.move(); + + // 可動範囲の制限を解除 + move_group_arm.clearPathConstraints(); + + // SRDFに定義されている"r_arm_waist_init_pose"の姿勢にする + move_group_arm.setNamedTarget("r_arm_waist_init_pose"); + move_group_arm.move(); + + // ハンドを閉じる + gripper_joint_values[0] = GRIPPER_CLOSE; + move_group_gripper.setJointValueTarget(gripper_joint_values); + move_group_gripper.move(); + + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_examples/src/pose_presets.cpp b/sciurus17_examples/src/pose_presets.cpp new file mode 100644 index 00000000..33b8714b --- /dev/null +++ b/sciurus17_examples/src/pose_presets.cpp @@ -0,0 +1,56 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_presets.hpp" +#include "angles/angles.h" +#include "geometry_msgs/msg/quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace pose_presets +{ +// Pose型の位置姿勢を作成 +geometry_msgs::msg::Pose generate_pose( + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw) +{ + geometry_msgs::msg::Pose target_pose; + tf2::Quaternion q; + target_pose.position.x = x; + target_pose.position.y = y; + target_pose.position.z = z; + q.setRPY(roll, pitch, yaw); + target_pose.orientation = tf2::toMsg(q); + return target_pose; +} + +// 右グリッパを下に向ける姿勢を作成 +geometry_msgs::msg::Pose right_arm_downward(const double x, const double y, const double z) +{ + geometry_msgs::msg::Pose target_pose; + target_pose = generate_pose( + x, y, z, + angles::from_degrees(90), angles::from_degrees(0), angles::from_degrees(0)); + return target_pose; +} + +// 左グリッパを下に向ける姿勢を作成 +geometry_msgs::msg::Pose left_arm_downward(const double x, const double y, const double z) +{ + geometry_msgs::msg::Pose target_pose; + target_pose = generate_pose( + x, y, z, + angles::from_degrees(-90), angles::from_degrees(0), angles::from_degrees(0)); + return target_pose; +} +} // namespace pose_presets diff --git a/sciurus17_examples/src/waist_control.cpp b/sciurus17_examples/src/waist_control.cpp new file mode 100644 index 00000000..1774f6d2 --- /dev/null +++ b/sciurus17_examples/src/waist_control.cpp @@ -0,0 +1,67 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +// examples/move_group_interface/src/move_group_interface_tutorial.cpp + +#include "angles/angles.h" +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("waist_control"); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("waist_control", node_options); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() {executor.spin();}).detach(); + + // 腰制御用MoveGroupInterface + MoveGroupInterface move_group_waist(move_group_node, "waist_group"); + // 駆動速度を調整する + move_group_waist.setMaxVelocityScalingFactor(0.1); // Set 0.0 ~ 1.0 + move_group_waist.setMaxAccelerationScalingFactor(0.1); // Set 0.0 ~ 1.0 + + // SRDFに定義されている"waist_init_pose"の姿勢にする + move_group_waist.setNamedTarget("waist_init_pose"); + move_group_waist.move(); + + // 現在角度をベースに、目標角度を作成する + auto joint_values = move_group_waist.getCurrentJointValues(); + + // 腰を左に向ける + joint_values[0] = angles::from_degrees(45.0); + move_group_waist.setJointValueTarget(joint_values); + move_group_waist.move(); + + // 腰を左に向ける + joint_values[0] = angles::from_degrees(-45.0); + move_group_waist.setJointValueTarget(joint_values); + move_group_waist.move(); + + // "waist_init_pose"に戻す + move_group_waist.setNamedTarget("waist_init_pose"); + move_group_waist.move(); + + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_gazebo/worlds/table.sdf b/sciurus17_gazebo/worlds/table.sdf index 51761347..95721110 100644 --- a/sciurus17_gazebo/worlds/table.sdf +++ b/sciurus17_gazebo/worlds/table.sdf @@ -1,8 +1,8 @@ - - 0.001 + + 0.005 1.0 https://fuel.gazebosim.org/1.0/OpenRobotics/models/Wood cube 5cm - 0.20 0 1.0 0 0 0 + 0.25 0 1.0 0 0 0 diff --git a/sciurus17_moveit_config/config/joint_limits.yaml b/sciurus17_moveit_config/config/joint_limits.yaml index 5fe344c1..864fad83 100644 --- a/sciurus17_moveit_config/config/joint_limits.yaml +++ b/sciurus17_moveit_config/config/joint_limits.yaml @@ -53,6 +53,16 @@ joint_limits: max_velocity: 5.9692114350000001 has_acceleration_limits: true max_acceleration: 10.0 + neck_pitch_joint: + has_velocity_limits: true + max_velocity: 5.9692114350000001 + has_acceleration_limits: true + max_acceleration: 10.0 + neck_yaw_joint: + has_velocity_limits: true + max_velocity: 5.9692114350000001 + has_acceleration_limits: true + max_acceleration: 10.0 r_arm_joint1: has_velocity_limits: true max_velocity: 5.9692114350000001 diff --git a/sciurus17_moveit_config/config/moveit_controllers.yaml b/sciurus17_moveit_config/config/moveit_controllers.yaml index e532b7af..c3afd9f3 100644 --- a/sciurus17_moveit_config/config/moveit_controllers.yaml +++ b/sciurus17_moveit_config/config/moveit_controllers.yaml @@ -1,5 +1,8 @@ # MoveIt uses this configuration for controller management +trajectory_execution: + allowed_start_tolerance: 0.1 + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager moveit_simple_controller_manager: diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index 33398a8f..b77822dc 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -59,6 +59,13 @@ + + + + + + + @@ -114,6 +121,13 @@ + + + + + + +