Fix run.launch.py to load planning pipelines

This commit is contained in:
Robert Haschke 2025-10-04 18:17:56 +02:00
parent 4a42072e96
commit 9f7a0ac195
2 changed files with 10 additions and 9 deletions

View File

@ -10,6 +10,7 @@ from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description(): def generate_launch_description():
moveit_config = ( moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda") MoveItConfigsBuilder("moveit_resources_panda")
.planning_pipelines(pipelines=["ompl"])
.robot_description(file_path="config/panda.urdf.xacro") .robot_description(file_path="config/panda.urdf.xacro")
.to_moveit_configs() .to_moveit_configs()
) )
@ -21,11 +22,7 @@ def generate_launch_description():
executable=LaunchConfiguration("exe"), executable=LaunchConfiguration("exe"),
output="screen", output="screen",
parameters=[ parameters=[
moveit_config.robot_description, moveit_config.to_dict(),
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
moveit_config.planning_pipelines,
os.path.join(package_shared_path, "config", "panda_config.yaml"), os.path.join(package_shared_path, "config", "panda_config.yaml"),
], ],
) )

View File

@ -11,10 +11,13 @@ import time
import rclcpp import rclcpp
rclcpp.init() rclcpp.init()
node = rclcpp.Node("mtc_tutorial")
node_options = rclcpp.NodeOptions()
node_options.automatically_declare_parameters_from_overrides = True
node = rclcpp.Node("mtc_tutorial", node_options)
group = "panda_arm" group = "panda_arm"
planner = core.PipelinePlanner(node) planner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
task = core.Task() task = core.Task()
task.name = "constrained" task.name = "constrained"
@ -50,7 +53,8 @@ pose.position.y = 0.0
pose.position.z = 0.485 pose.position.z = 0.485
pose.orientation.x = pose.orientation.w = 0.70711 # 90° about x pose.orientation.x = pose.orientation.w = 0.70711 # 90° about x
mps.addObject(co) mps.addObject(co)
mps.attachObjects(["object"], "panda_hand") mps.attachObjects(["object"], "panda_link8")
mps.allowCollisions("object", ["panda_leftfinger", "panda_rightfinger"], True)
task.add(mps) task.add(mps)
@ -77,4 +81,4 @@ task.add(move)
if task.plan(): if task.plan():
task.publish(task.solutions[0]) task.publish(task.solutions[0])
time.sleep(100) time.sleep(500)