Skip to content

Commit

Permalink
flake8通るように修正
Browse files Browse the repository at this point in the history
  • Loading branch information
@chama1176
chama1176 committed Nov 28, 2024
1 parent ab8fb76 commit fe33346
Show file tree
Hide file tree
Showing 5 changed files with 95 additions and 133 deletions.
66 changes: 20 additions & 46 deletions sciurus17_examples_py/launch/example.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,78 +12,52 @@
# 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
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch
from moveit_configs_utils.launches import generate_moveit_rviz_launch
from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
from moveit_configs_utils.launches import generate_rsp_launch
from sciurus17_description.robot_description_loader import RobotDescriptionLoader
from ament_index_python.packages import get_package_share_directory


from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
ld = LaunchDescription()
description_loader = RobotDescriptionLoader()

ld.add_action(
DeclareLaunchArgument(
'loaded_description',
default_value=description_loader.load(),
description='Set robot_description text. \
It is recommended to use RobotDescriptionLoader() in sciurus17_description.'
)
)
DeclareLaunchArgument('loaded_description',
default_value=description_loader.load(),
description='Set robot_description text. \
It is recommended to use RobotDescriptionLoader() \
in sciurus17_description.'))

moveit_config = (
MoveItConfigsBuilder("sciurus17")
.planning_scene_monitor(
publish_robot_description=True,
publish_robot_description_semantic=True,
)
.trajectory_execution(file_path="config/moveit_controllers.yaml")
.planning_pipelines(pipelines=["ompl"])
.moveit_cpp(
file_path=get_package_share_directory("sciurus17_examples_py")
+ "/config/sciurus17_moveit_py_examples.yaml"
)
.to_moveit_configs()
)
moveit_config = (MoveItConfigsBuilder('sciurus17').planning_scene_monitor(
publish_robot_description=True,
publish_robot_description_semantic=True,
).trajectory_execution(file_path='config/moveit_controllers.yaml').planning_pipelines(
pipelines=['ompl']).moveit_cpp(
file_path=get_package_share_directory('sciurus17_examples_py') +
'/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs())

moveit_config.robot_description = {
'robot_description': LaunchConfiguration('loaded_description')
}
}

moveit_config.move_group_capabilities = {
"capabilities": ""
}
moveit_config.move_group_capabilities = {'capabilities': ''}

declare_example_name = DeclareLaunchArgument(
'example', default_value='gripper_control',
'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]')
)
'pick_and_place_right_arm_waist, pick_and_place_left_arm]'))

example_node = Node(
name=[LaunchConfiguration('example'), '_node'],
package='sciurus17_examples_py',
executable=LaunchConfiguration('example'),
output='screen',
package='sciurus17_examples_py',
executable=LaunchConfiguration('example'),
output='screen',
parameters=[moveit_config.to_dict()],
)

Expand Down
46 changes: 21 additions & 25 deletions sciurus17_examples_py/sciurus17_examples_py/gripper_control.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,67 +11,63 @@
# 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 time
import math
import copy

# generic ros libraries
import rclpy
from rclpy.logging import get_logger
from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
from std_msgs.msg import Header

# moveit python library
from moveit.core.robot_state import RobotState
from moveit.planning import (
MoveItPy,
PlanRequestParameters,
)
# generic ros libraries
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)
logger = get_logger("moveit_py.gripper_control")
logger = get_logger('moveit_py.gripper_control')

# instantiate MoveItPy instance and get planning component
sciurus17 = MoveItPy(node_name="gripper_control")
logger.info("MoveItPy instance created")
sciurus17 = MoveItPy(node_name='gripper_control')
logger.info('MoveItPy instance created')

# 腕制御用 planning component
arm = sciurus17.get_planning_component("two_arm_group")
arm = sciurus17.get_planning_component('two_arm_group')
# 左グリッパ制御用 planning component
l_gripper = sciurus17.get_planning_component("l_gripper_group")
l_gripper = sciurus17.get_planning_component('l_gripper_group')
# 右グリッパ制御用 planning component
r_gripper = sciurus17.get_planning_component("r_gripper_group")
r_gripper = sciurus17.get_planning_component('r_gripper_group')

robot_model = sciurus17.get_robot_model()

plan_request_params = PlanRequestParameters(
sciurus17,
"ompl_rrtc_default",
'ompl_rrtc_default',
)
gripper_plan_request_params = PlanRequestParameters(
sciurus17,
"ompl_rrtc_default",
'ompl_rrtc_default',
)
# 動作速度の調整
plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0
plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0
plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0
plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0
# 動作速度の調整
gripper_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0
gripper_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0
gripper_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0
gripper_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0

# グリッパの開閉角
R_GRIPPER_CLOSE = math.radians(0.0)
R_GRIPPER_OPEN = math.radians(40.0)
L_GRIPPER_CLOSE = math.radians(0.0)
L_GRIPPER_OPEN = math.radians(-40.0)

# SRDFに定義されている"two_arm_init_pose"の姿勢にする
# SRDFに定義されている'two_arm_init_pose'の姿勢にする
arm.set_start_state_to_current_state()
arm.set_goal_state(configuration_name="two_arm_init_pose")
arm.set_goal_state(configuration_name='two_arm_init_pose')
plan_and_execute(
sciurus17,
arm,
Expand All @@ -82,7 +78,7 @@ def main(args=None):
for _ in range(2):
r_gripper.set_start_state_to_current_state()
robot_state = RobotState(robot_model)
robot_state.set_joint_group_positions("r_gripper_group", [R_GRIPPER_OPEN])
robot_state.set_joint_group_positions('r_gripper_group', [R_GRIPPER_OPEN])
r_gripper.set_goal_state(robot_state=robot_state)
plan_and_execute(
sciurus17,
Expand All @@ -93,7 +89,7 @@ def main(args=None):

r_gripper.set_start_state_to_current_state()
robot_state = RobotState(robot_model)
robot_state.set_joint_group_positions("r_gripper_group", [R_GRIPPER_CLOSE])
robot_state.set_joint_group_positions('r_gripper_group', [R_GRIPPER_CLOSE])
r_gripper.set_goal_state(robot_state=robot_state)
plan_and_execute(
sciurus17,
Expand All @@ -105,7 +101,7 @@ def main(args=None):
for _ in range(2):
l_gripper.set_start_state_to_current_state()
robot_state = RobotState(robot_model)
robot_state.set_joint_group_positions("l_gripper_group", [L_GRIPPER_OPEN])
robot_state.set_joint_group_positions('l_gripper_group', [L_GRIPPER_OPEN])
l_gripper.set_goal_state(robot_state=robot_state)
plan_and_execute(
sciurus17,
Expand All @@ -116,7 +112,7 @@ def main(args=None):

l_gripper.set_start_state_to_current_state()
robot_state = RobotState(robot_model)
robot_state.set_joint_group_positions("l_gripper_group", [L_GRIPPER_CLOSE])
robot_state.set_joint_group_positions('l_gripper_group', [L_GRIPPER_CLOSE])
l_gripper.set_goal_state(robot_state=robot_state)
plan_and_execute(
sciurus17,
Expand Down
Loading

0 comments on commit fe33346

Please sign in to comment.