From 3758f519f26354dc6ed2f5aad6cb398b61264c9b Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 8 Jul 2025 14:43:20 +0900 Subject: [PATCH 01/17] =?UTF-8?q?c++=E3=81=AEpose=5Fgroupstate=E3=82=92?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 1 + sciurus17_examples/launch/example.launch.py | 2 +- sciurus17_examples/src/pose_groupstate.cpp | 53 +++++++++++++++++++ sciurus17_moveit_config/config/sciurus17.srdf | 36 ++++++++++++- 4 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 sciurus17_examples/src/pose_groupstate.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index d4ed8da6..7b51d925 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -113,6 +113,7 @@ install( # Build and install node executables set(executable_list gripper_control + pose_groupstate neck_control waist_control pick_and_place_right_arm_waist diff --git a/sciurus17_examples/launch/example.launch.py b/sciurus17_examples/launch/example.launch.py index b2fd1206..b95ee44f 100644 --- a/sciurus17_examples/launch/example.launch.py +++ b/sciurus17_examples/launch/example.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): declare_example_name = DeclareLaunchArgument( 'example', default_value='gripper_control', description=('Set an example executable name: ' - '[gripper_control, neck_control, waist_control,' + '[gripper_control, pose_groupstate, neck_control, waist_control,' 'pick_and_place_right_arm_waist, pick_and_place_left_arm]') ) diff --git a/sciurus17_examples/src/pose_groupstate.cpp b/sciurus17_examples/src/pose_groupstate.cpp new file mode 100644 index 00000000..d0cfb003 --- /dev/null +++ b/sciurus17_examples/src/pose_groupstate.cpp @@ -0,0 +1,53 @@ +// Copyright 2022 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 +// /5c15da709e9ea8529b54b313dc570f164f9a713e/doc/examples/subframes +// /src/subframes_tutorial.cpp + +#include "moveit/move_group_interface/move_group_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("pose_groupstate"); + +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); + // For current state monitor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_arm_node); + std::thread([&executor]() {executor.spin();}).detach(); + + MoveGroupInterface move_group_arm(move_group_arm_node, "two_arm_group"); + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + + move_group_arm.setNamedTarget("two_arm_init_pose"); + move_group_arm.move(); + + move_group_arm.setNamedTarget("two_arm_horizontal_pose"); + move_group_arm.move(); + + move_group_arm.setNamedTarget("two_arm_init_pose"); + move_group_arm.move(); + + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index b77822dc..2bbecb4e 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -76,6 +76,15 @@ + + + + + + + + + @@ -85,9 +94,18 @@ + + + + + + + + + - + @@ -121,6 +139,22 @@ + + + + + + + + + + + + + + + + From 4ef666dad37d2efca4ed7d0cd8fa609f3e8c5612 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 8 Jul 2025 17:29:21 +0900 Subject: [PATCH 02/17] =?UTF-8?q?group=5Fstate=E3=81=AEPython=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 2 +- .../sciurus17_examples_py/pose_groupstate.py | 82 +++++++++++++++++++ sciurus17_examples_py/setup.py | 1 + 3 files changed, 84 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 4c560243..b741bbe5 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): 'example', default_value='gripper_control', description=('Set an example executable name: ' - '[gripper_control, neck_control, waist_control, ' + '[gripper_control, pose_groupstate, neck_control, waist_control, ' 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) declare_use_sim_time = DeclareLaunchArgument( diff --git a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py new file mode 100644 index 00000000..7babf5d9 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py @@ -0,0 +1,82 @@ +# Copyright 2025 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. + +from sciurus17_examples_py.utils import plan_and_execute + +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('pose_groupstate') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='pose_groupstate') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = sciurus17.get_planning_component('two_arm_group') + + arm_plan_request_params = PlanRequestParameters( + sciurus17, + 'ompl_rrtc_default', + ) + + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # SRDFに定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='two_arm_init_pose') + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDFに定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='two_arm_horizontal_pose') + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDFに定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='two_arm_init_pose') + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 8ee83fb0..9ffc05a3 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -31,6 +31,7 @@ 'pick_and_place_right_arm_waist = \ sciurus17_examples_py.pick_and_place_right_arm_waist:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + 'pose_groupstate = sciurus17_examples_py.pose_groupstate:main', ], }, ) From 0a4ec12e89b8e9252de161f4f7ae312b9699c15a Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 8 Jul 2025 18:18:22 +0900 Subject: [PATCH 03/17] =?UTF-8?q?pose=5Fgroup=E3=81=AE=E5=91=BD=E5=90=8D?= =?UTF-8?q?=E3=82=92=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/pose_groupstate.cpp | 4 +-- .../sciurus17_examples_py/pose_groupstate.py | 2 +- sciurus17_moveit_config/config/sciurus17.srdf | 26 +++++++++---------- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/sciurus17_examples/src/pose_groupstate.cpp b/sciurus17_examples/src/pose_groupstate.cpp index d0cfb003..bca3d95a 100644 --- a/sciurus17_examples/src/pose_groupstate.cpp +++ b/sciurus17_examples/src/pose_groupstate.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 RT Corporation +// Copyright 2025 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,7 +42,7 @@ int main(int argc, char ** argv) move_group_arm.setNamedTarget("two_arm_init_pose"); move_group_arm.move(); - move_group_arm.setNamedTarget("two_arm_horizontal_pose"); + move_group_arm.setNamedTarget("two_arm_push_forward_pose"); move_group_arm.move(); move_group_arm.setNamedTarget("two_arm_init_pose"); diff --git a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py index 7babf5d9..20620e2b 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py @@ -55,7 +55,7 @@ def main(args=None): # SRDFに定義されている'vertical'の姿勢にする arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='two_arm_horizontal_pose') + arm.set_goal_state(configuration_name='two_arm_push_forward_pose') plan_and_execute( sciurus17, arm, diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index 2bbecb4e..7b106380 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -76,11 +76,11 @@ - - + + - + @@ -94,7 +94,7 @@ - + @@ -139,20 +139,20 @@ - - - + + + - + - + - - + + - + - + From 76b60b009322b20c04b825ac77c0e7e28528f724 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 9 Jul 2025 13:52:36 +0900 Subject: [PATCH 04/17] =?UTF-8?q?=E3=83=97=E3=83=AD=E3=82=B0=E3=83=A9?= =?UTF-8?q?=E3=83=A0=E4=B8=AD=E3=81=AE=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples_py/pose_groupstate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py index 20620e2b..bc6a5997 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py @@ -43,7 +43,7 @@ def main(args=None): arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # SRDFに定義されている'home'の姿勢にする + # SRDFに定義されている'two_arm_init_pose'の姿勢にする arm.set_start_state_to_current_state() arm.set_goal_state(configuration_name='two_arm_init_pose') plan_and_execute( @@ -53,7 +53,7 @@ def main(args=None): single_plan_parameters=arm_plan_request_params, ) - # SRDFに定義されている'vertical'の姿勢にする + # SRDFに定義されている'two_arm_push_forward_pose'の姿勢にする arm.set_start_state_to_current_state() arm.set_goal_state(configuration_name='two_arm_push_forward_pose') plan_and_execute( @@ -63,7 +63,7 @@ def main(args=None): single_plan_parameters=arm_plan_request_params, ) - # SRDFに定義されている'home'の姿勢にする + # SRDFに定義されている'two_arm_init_pose'の姿勢にする arm.set_start_state_to_current_state() arm.set_goal_state(configuration_name='two_arm_init_pose') plan_and_execute( From 8692df8591615a59646082d90881cbaddcb7cadd Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Thu, 10 Jul 2025 12:34:23 +0900 Subject: [PATCH 05/17] =?UTF-8?q?joint=5Fvalues=E3=81=AEPython=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E3=82=92=E7=A7=BB=E6=A4=8D=EF=BC=88?= =?UTF-8?q?=E8=A7=92=E5=BA=A6=E6=9C=AA=E8=AA=BF=E6=95=B4=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 2 +- .../sciurus17_examples_py/joint_values.py | 109 ++++++++++++++++++ sciurus17_examples_py/setup.py | 1 + 3 files changed, 111 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples_py/sciurus17_examples_py/joint_values.py diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index b741bbe5..398f7712 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): default_value='gripper_control', description=('Set an example executable name: ' '[gripper_control, pose_groupstate, neck_control, waist_control, ' - 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) + 'pick_and_place_right_arm_waist, pick_and_place_left_arm, joint_values]')) declare_use_sim_time = DeclareLaunchArgument( 'use_sim_time', default_value='false', diff --git a/sciurus17_examples_py/sciurus17_examples_py/joint_values.py b/sciurus17_examples_py/sciurus17_examples_py/joint_values.py new file mode 100644 index 00000000..99129ca4 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/joint_values.py @@ -0,0 +1,109 @@ +# Copyright 2025 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 math + +from sciurus17_examples_py.utils import plan_and_execute + +from moveit.core.kinematic_constraints import construct_joint_constraint +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('joint_values') + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name='joint_values') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = sciurus17.get_planning_component('l_arm_group') + + # instantiate a RobotState instance using the current robot model + robot_model = sciurus17.get_robot_model() + robot_state = RobotState(robot_model) + + arm_plan_request_params = PlanRequestParameters( + sciurus17, + 'ompl_rrtc_default', + ) + + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 0.5 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 0.5 # Set 0.0 ~ 1.0 + + # SRDFに定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + joint_names = [ + 'l_arm_joint1', + 'l_arm_joint2', + 'l_arm_joint3', + 'l_arm_joint4', + 'l_arm_joint5', + 'l_arm_joint6', + ] + target_joint_value = math.radians(-45.0) + + # 各関節角度を順番に-45[deg]ずつ動かす + for joint_name in joint_names: + arm.set_start_state_to_current_state() + + joint_values = {joint_name: target_joint_value} + robot_state.joint_positions = joint_values + joint_constraint = construct_joint_constraint( + robot_state=robot_state, + joint_model_group=sciurus17.get_robot_model().get_joint_model_group('l_arm_group'), + ) + arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDFに定義されている'l_arm_init_pose'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='l_arm_init_pose') + plan_and_execute( + sciurus17, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 9ffc05a3..566c0411 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -32,6 +32,7 @@ sciurus17_examples_py.pick_and_place_right_arm_waist:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', 'pose_groupstate = sciurus17_examples_py.pose_groupstate:main', + 'joint_values = sciurus17_examples_py.joint_values:main', ], }, ) From 3f08a75123dda13630c612bac92008f023c5661e Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Thu, 10 Jul 2025 14:27:38 +0900 Subject: [PATCH 06/17] =?UTF-8?q?Revert=20"joint=5Fvalues=E3=81=AEPython?= =?UTF-8?q?=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E3=82=92=E7=A7=BB=E6=A4=8D?= =?UTF-8?q?=EF=BC=88=E8=A7=92=E5=BA=A6=E6=9C=AA=E8=AA=BF=E6=95=B4=EF=BC=89?= =?UTF-8?q?"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 8692df8591615a59646082d90881cbaddcb7cadd. --- .../launch/example.launch.py | 2 +- .../sciurus17_examples_py/joint_values.py | 109 ------------------ sciurus17_examples_py/setup.py | 1 - 3 files changed, 1 insertion(+), 111 deletions(-) delete mode 100644 sciurus17_examples_py/sciurus17_examples_py/joint_values.py diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 398f7712..b741bbe5 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): default_value='gripper_control', description=('Set an example executable name: ' '[gripper_control, pose_groupstate, neck_control, waist_control, ' - 'pick_and_place_right_arm_waist, pick_and_place_left_arm, joint_values]')) + 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) declare_use_sim_time = DeclareLaunchArgument( 'use_sim_time', default_value='false', diff --git a/sciurus17_examples_py/sciurus17_examples_py/joint_values.py b/sciurus17_examples_py/sciurus17_examples_py/joint_values.py deleted file mode 100644 index 99129ca4..00000000 --- a/sciurus17_examples_py/sciurus17_examples_py/joint_values.py +++ /dev/null @@ -1,109 +0,0 @@ -# Copyright 2025 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 math - -from sciurus17_examples_py.utils import plan_and_execute - -from moveit.core.kinematic_constraints import construct_joint_constraint -from moveit.core.robot_state import RobotState -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) - -import rclpy -from rclpy.logging import get_logger - - -def main(args=None): - rclpy.init(args=args) - logger = get_logger('joint_values') - - # instantiate MoveItPy instance and get planning component - sciurus17 = MoveItPy(node_name='joint_values') - logger.info('MoveItPy instance created') - - # アーム制御用 planning component - arm = sciurus17.get_planning_component('l_arm_group') - - # instantiate a RobotState instance using the current robot model - robot_model = sciurus17.get_robot_model() - robot_state = RobotState(robot_model) - - arm_plan_request_params = PlanRequestParameters( - sciurus17, - 'ompl_rrtc_default', - ) - - # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 0.5 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 0.5 # Set 0.0 ~ 1.0 - - # SRDFに定義されている'vertical'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='vertical') - plan_and_execute( - sciurus17, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - joint_names = [ - 'l_arm_joint1', - 'l_arm_joint2', - 'l_arm_joint3', - 'l_arm_joint4', - 'l_arm_joint5', - 'l_arm_joint6', - ] - target_joint_value = math.radians(-45.0) - - # 各関節角度を順番に-45[deg]ずつ動かす - for joint_name in joint_names: - arm.set_start_state_to_current_state() - - joint_values = {joint_name: target_joint_value} - robot_state.joint_positions = joint_values - joint_constraint = construct_joint_constraint( - robot_state=robot_state, - joint_model_group=sciurus17.get_robot_model().get_joint_model_group('l_arm_group'), - ) - arm.set_goal_state(motion_plan_constraints=[joint_constraint]) - - plan_and_execute( - sciurus17, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # SRDFに定義されている'l_arm_init_pose'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='l_arm_init_pose') - plan_and_execute( - sciurus17, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # Finish with error. Related Issue - # https://github.com/moveit/moveit2/issues/2693 - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 566c0411..9ffc05a3 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -32,7 +32,6 @@ sciurus17_examples_py.pick_and_place_right_arm_waist:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', 'pose_groupstate = sciurus17_examples_py.pose_groupstate:main', - 'joint_values = sciurus17_examples_py.joint_values:main', ], }, ) From a0b514843ba15263bb30a50735414cb96a2319f6 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Fri, 11 Jul 2025 14:08:07 +0900 Subject: [PATCH 07/17] =?UTF-8?q?=E6=96=B0=E3=81=9F=E3=81=AB=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0=E3=81=97=E3=81=9F=E3=82=B3=E3=83=BC=E3=83=89=E3=81=AE?= =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88=E3=82=92?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/pose_groupstate.cpp | 4 ++-- .../sciurus17_examples_py/pose_groupstate.py | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/sciurus17_examples/src/pose_groupstate.cpp b/sciurus17_examples/src/pose_groupstate.cpp index bca3d95a..3879f6ec 100644 --- a/sciurus17_examples/src/pose_groupstate.cpp +++ b/sciurus17_examples/src/pose_groupstate.cpp @@ -33,10 +33,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); - std::thread([&executor]() {executor.spin();}).detach(); + std::thread([&executor]() { executor.spin(); }).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "two_arm_group"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setNamedTarget("two_arm_init_pose"); diff --git a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py index bc6a5997..b66692c8 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py @@ -12,16 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from sciurus17_examples_py.utils import plan_and_execute - -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) +from moveit.planning import MoveItPy +from moveit.planning import PlanRequestParameters import rclpy from rclpy.logging import get_logger +from sciurus17_examples_py.utils import plan_and_execute + def main(args=None): rclpy.init(args=args) @@ -40,7 +38,9 @@ def main(args=None): ) # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = ( + 1.0 # Set 0.0 ~ 1.0 + ) arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # SRDFに定義されている'two_arm_init_pose'の姿勢にする From 3b6c23d962ce598d62bf7678e952fc933db725ed Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Fri, 11 Jul 2025 14:37:36 +0900 Subject: [PATCH 08/17] =?UTF-8?q?ament=5Funcrustify=E3=81=AE=E3=83=95?= =?UTF-8?q?=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88=E3=82=92=E9=81=A9?= =?UTF-8?q?=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/pose_groupstate.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/pose_groupstate.cpp b/sciurus17_examples/src/pose_groupstate.cpp index 3879f6ec..2104ded9 100644 --- a/sciurus17_examples/src/pose_groupstate.cpp +++ b/sciurus17_examples/src/pose_groupstate.cpp @@ -33,7 +33,7 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); - std::thread([&executor]() { executor.spin(); }).detach(); + std::thread([&executor]() {executor.spin();}).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "two_arm_group"); move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 From 86b0af2224b26e289897887c832b928e4f7bb9b5 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 15 Jul 2025 17:43:03 +0900 Subject: [PATCH 09/17] =?UTF-8?q?README=E3=81=AB=E6=89=8B=E9=A0=86?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 17 +++++++++++++++++ sciurus17_examples_py/README.md | 13 +++++++++++++ 2 files changed, 30 insertions(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index e585da49..f16b6182 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -103,6 +103,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ `demo.launch`を実行している状態で各サンプルを実行できます。 - [gripper\_control](#gripper_control) +- [pose\_groustate](#pose_groupstate) - [neck\_control](#neck_control) - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) @@ -131,6 +132,22 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' --- +### pose_groupstate + +group_stateを使うコード例です。 + +SRDFファイル[sciurus17_moveit_config/config/sciurus17.srdf](../sciurus17_moveit_config/config/sciurus17.srdf)に記載されている`two_arm_init_pose`と`two_arm_push_forward_pose`の姿勢に移行します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='pose_groupstate' +``` + +[back to example list](#examples) + +--- + ### neck_control 首を上下左右へ動かすコード例です。 diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 78f2f77c..27470dff 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -40,6 +40,7 @@ ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' u `demo.launch`を実行している状態で各サンプルを実行できます。 - [gripper\_control](#gripper_control) +- [pose\_groustate](#pose_groupstate) - [neck\_control](#neck_control) - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) @@ -65,6 +66,18 @@ ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' --- +### pose_groupstate + +group_stateを使うコード例です。 + +SRDFファイル[sciurus17_moveit_config/config/sciurus17.srdf](../sciurus17_moveit_config/config/sciurus17.srdf)に記載されている`two_arm_init_pose`と`two_arm_push_forward_pose`の姿勢に移行します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples example.launch.py example:='pose_groupstate' +``` + ### neck_control 首を上下左右へ動かすコード例です。 From 5f1e671629e07d83668a66050acd06d719602fcb Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 22 Jul 2025 14:49:37 +0900 Subject: [PATCH 10/17] Update sciurus17_examples_py/README.md Co-authored-by: Kuwamai --- sciurus17_examples_py/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 27470dff..756108e5 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -75,7 +75,7 @@ SRDFファイル[sciurus17_moveit_config/config/sciurus17.srdf](../sciurus17_mov 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples example.launch.py example:='pose_groupstate' +ros2 launch sciurus17_examples_py example.launch.py example:='pose_groupstate' ``` ### neck_control From edf435c10d61ef49c70839354967fe2e77ca0986 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 22 Jul 2025 14:49:49 +0900 Subject: [PATCH 11/17] Update sciurus17_examples_py/README.md Co-authored-by: Kuwamai --- sciurus17_examples_py/README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 756108e5..d081d070 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -78,6 +78,10 @@ SRDFファイル[sciurus17_moveit_config/config/sciurus17.srdf](../sciurus17_mov ros2 launch sciurus17_examples_py example.launch.py example:='pose_groupstate' ``` +[back to example list](#examples) + +--- + ### neck_control 首を上下左右へ動かすコード例です。 From cb95ae4ba0f4180a399f89f049d1c1329ec86b63 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 22 Jul 2025 14:56:06 +0900 Subject: [PATCH 12/17] Update sciurus17_moveit_config/config/sciurus17.srdf Co-authored-by: Kuwamai --- sciurus17_moveit_config/config/sciurus17.srdf | 9 --------- 1 file changed, 9 deletions(-) diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index 7b106380..47b9c2b6 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -76,15 +76,6 @@ - - - - - - - - - From 8560ae795cf03af13d82880111b2d58ae20e88c4 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 22 Jul 2025 14:56:35 +0900 Subject: [PATCH 13/17] Update sciurus17_moveit_config/config/sciurus17.srdf Co-authored-by: Kuwamai --- sciurus17_moveit_config/config/sciurus17.srdf | 9 --------- 1 file changed, 9 deletions(-) diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index 47b9c2b6..62b6695a 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -85,15 +85,6 @@ - - - - - - - - - From 97dc03c12d0a7317e9833bb740c537b1a46f1145 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 22 Jul 2025 15:08:30 +0900 Subject: [PATCH 14/17] =?UTF-8?q?=E7=AA=81=E3=81=8D=E5=87=BA=E3=81=99?= =?UTF-8?q?=E3=82=A2=E3=83=BC=E3=83=A0=E3=81=AEjoint=E3=82=92=E5=B0=8F?= =?UTF-8?q?=E3=81=95=E3=81=8F=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Kuwamai --- sciurus17_moveit_config/config/sciurus17.srdf | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index 62b6695a..0bd241ed 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -125,16 +125,16 @@ - + - + - + - + From 191710b6c671871d084127f0ce2994c9d46551ee Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 22 Jul 2025 15:12:45 +0900 Subject: [PATCH 15/17] =?UTF-8?q?init=5Fpose=E3=81=AE=E3=83=9F=E3=82=B9?= =?UTF-8?q?=E3=81=AB=E3=82=88=E3=82=8B=E6=95=B0=E5=80=A4=E5=A4=89=E6=9B=B4?= =?UTF-8?q?=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_moveit_config/config/sciurus17.srdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_moveit_config/config/sciurus17.srdf b/sciurus17_moveit_config/config/sciurus17.srdf index 0bd241ed..10cbdaa1 100644 --- a/sciurus17_moveit_config/config/sciurus17.srdf +++ b/sciurus17_moveit_config/config/sciurus17.srdf @@ -87,7 +87,7 @@ - + From 9cf65e10c757b8ead40768d2044d0b0e9d71c0c8 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 22 Jul 2025 15:18:04 +0900 Subject: [PATCH 16/17] =?UTF-8?q?=E3=82=A2=E3=83=BC=E3=83=A0=E3=81=AE?= =?UTF-8?q?=E5=8A=A0=E9=80=9F=E5=BA=A6=E3=81=A8=E9=80=9F=E5=BA=A6=E8=AA=BF?= =?UTF-8?q?=E6=95=B4=E7=AE=87=E6=89=80=E3=81=AE=E3=83=AC=E3=82=A4=E3=82=A2?= =?UTF-8?q?=E3=82=A6=E3=83=88=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Kuwamai --- .../sciurus17_examples_py/pose_groupstate.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py index b66692c8..036c21fb 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pose_groupstate.py @@ -38,10 +38,9 @@ def main(args=None): ) # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = ( - 1.0 # Set 0.0 ~ 1.0 - ) - arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # SRDFに定義されている'two_arm_init_pose'の姿勢にする arm.set_start_state_to_current_state() From 6ced66c3bdd1bc42d9fdec6ecdac1483138dc013 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 22 Jul 2025 15:24:46 +0900 Subject: [PATCH 17/17] =?UTF-8?q?=E5=86=92=E9=A0=AD=E3=81=AE=E7=9B=AE?= =?UTF-8?q?=E6=AC=A1=E3=81=AB=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E9=A0=85=E7=9B=AE=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 1 + sciurus17_examples_py/README.md | 1 + 2 files changed, 2 insertions(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index f16b6182..5990080b 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -15,6 +15,7 @@ - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) - [gripper\_control](#gripper_control) + - [pose\_groustate](#pose_groupstate) - [neck\_control](#neck_control) - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index d081d070..75ccd822 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -8,6 +8,7 @@ - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) - [gripper\_control](#gripper_control) + - [pose\_groustate](#pose_groupstate) - [neck\_control](#neck_control) - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)