mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Fix run.launch.py to load planning pipelines
This commit is contained in:
parent
4a42072e96
commit
9f7a0ac195
@ -10,6 +10,7 @@ from moveit_configs_utils import MoveItConfigsBuilder
|
||||
def generate_launch_description():
|
||||
moveit_config = (
|
||||
MoveItConfigsBuilder("moveit_resources_panda")
|
||||
.planning_pipelines(pipelines=["ompl"])
|
||||
.robot_description(file_path="config/panda.urdf.xacro")
|
||||
.to_moveit_configs()
|
||||
)
|
||||
@ -21,11 +22,7 @@ def generate_launch_description():
|
||||
executable=LaunchConfiguration("exe"),
|
||||
output="screen",
|
||||
parameters=[
|
||||
moveit_config.robot_description,
|
||||
moveit_config.robot_description_semantic,
|
||||
moveit_config.robot_description_kinematics,
|
||||
moveit_config.joint_limits,
|
||||
moveit_config.planning_pipelines,
|
||||
moveit_config.to_dict(),
|
||||
os.path.join(package_shared_path, "config", "panda_config.yaml"),
|
||||
],
|
||||
)
|
||||
|
||||
@ -11,10 +11,13 @@ import time
|
||||
import rclcpp
|
||||
|
||||
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"
|
||||
planner = core.PipelinePlanner(node)
|
||||
planner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
||||
|
||||
task = core.Task()
|
||||
task.name = "constrained"
|
||||
@ -50,7 +53,8 @@ pose.position.y = 0.0
|
||||
pose.position.z = 0.485
|
||||
pose.orientation.x = pose.orientation.w = 0.70711 # 90° about x
|
||||
mps.addObject(co)
|
||||
mps.attachObjects(["object"], "panda_hand")
|
||||
mps.attachObjects(["object"], "panda_link8")
|
||||
mps.allowCollisions("object", ["panda_leftfinger", "panda_rightfinger"], True)
|
||||
|
||||
task.add(mps)
|
||||
|
||||
@ -77,4 +81,4 @@ task.add(move)
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
time.sleep(100)
|
||||
time.sleep(500)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user