diff --git a/rasmt_moveit_config/config/joint_limits.yaml b/rasmt_moveit_config/config/joint_limits.yaml index 04f44ce..111f79e 100644 --- a/rasmt_moveit_config/config/joint_limits.yaml +++ b/rasmt_moveit_config/config/joint_limits.yaml @@ -2,8 +2,8 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1 +default_acceleration_scaling_factor: 1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] diff --git a/rasmt_moveit_config/config/rasmt_moveit.rviz b/rasmt_moveit_config/config/rasmt_moveit.rviz index a695e51..ac60364 100644 --- a/rasmt_moveit_config/config/rasmt_moveit.rviz +++ b/rasmt_moveit_config/config/rasmt_moveit.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 787 + Tree Height: 389 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -63,67 +63,13 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - rasmt_Base_Link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Dock_Link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Fork_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Fork_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Fork_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Grip_Body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Grip_L: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Grip_R: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rasmt_Link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true Value: false - - Acceleration_Scaling_Factor: 0.1 + - Acceleration_Scaling_Factor: 1 Class: moveit_rviz_plugin/MotionPlanning - Enabled: false + Enabled: true Move Group Namespace: "" MoveIt_Allow_Approximate_IK: false MoveIt_Allow_External_Program: false @@ -310,8 +256,8 @@ Visualization Manager: Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true - Value: false - Velocity_Scaling_Factor: 0.1 + Value: true + Velocity_Scaling_Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -376,16 +322,16 @@ Visualization Manager: Yaw: 5.318596363067627 Saved: ~ Window Geometry: - "": - collapsed: false - " - Trajectory Slider": - collapsed: false Displays: collapsed: false Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020a000001d10000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fbffffffff0000000253000001880000018800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000042a0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000042a0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: diff --git a/robossembler/config/params.yaml b/robossembler/config/params.yaml index c241d20..3e5b2ea 100644 --- a/robossembler/config/params.yaml +++ b/robossembler/config/params.yaml @@ -2,7 +2,7 @@ move: ros__parameters: plugins: - robossembler_move_bt_node - arm_group: ["rasms_arm_group"] + arm_group: ["rasmt_arm_group"] waypoints: ["one"] waypoint_coords: one: [0.01, 0.01, 0.6, diff --git a/robossembler/launch/robossembler_bringup.launch.py b/robossembler/launch/robossembler_bringup.launch.py index cfc3280..7c033d2 100644 --- a/robossembler/launch/robossembler_bringup.launch.py +++ b/robossembler/launch/robossembler_bringup.launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): # get error if xacro file if missing assert os.path.exists(xacro_file), "The xacro file of neptun_description doesnt exist"+str(xacro_file) # parse xacro file from file with arguments - robot_description = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"false",'robot_name':"rasmt"}) + robot_description = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"true",'robot_name':"rasmt"}) # convert file to xml format robot_description_content = robot_description.toxml() @@ -83,10 +83,23 @@ def generate_launch_description(): ] ) + task_planner_init = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("robossembler"), + "launch", + "task_planner.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content) + ] + ) + launch_nodes = [] launch_nodes.append(control) launch_nodes.append(simulation) launch_nodes.append(moveit) + launch_nodes.append(task_planner_init) return LaunchDescription(launch_args + launch_nodes) diff --git a/robossembler/launch/task_planner.launch.py b/robossembler/launch/task_planner.launch.py index 260b4b4..fb40cbc 100644 --- a/robossembler/launch/task_planner.launch.py +++ b/robossembler/launch/task_planner.launch.py @@ -33,34 +33,35 @@ def load_yaml(package_name, file_path): def generate_launch_description(): - # planning_context - robot_description_config = load_file( - "rasms_description", "urdf/rasms_description.urdf.xacro" - ) - robot_description = {"robot_description": robot_description_config} - robot_description_semantic_config = load_file( - "rasms_moveit_config", "srdf/rasms_description.srdf" + "rasmt_moveit_config", "config/rasmt.srdf" ) robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } kinematics_yaml = load_yaml( - "rasms_moveit_config", "config/kinematics.yml" + "rasmt_moveit_config", "config/kinematics.yaml" ) - pkg_dir = get_package_share_directory('task_planner') + pkg_dir = get_package_share_directory('robossembler') namespace = LaunchConfiguration('namespace') declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', + name="namespace", default_value='', description='Namespace') + declare_robot_description = DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file" + ) + # stdout_linebuf_envvar = SetEnvironmentVariable( # 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') + robot_description = {"robot_description":LaunchConfiguration("robot_description")} + plansys2_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('plansys2_bringup'), @@ -89,7 +90,7 @@ def generate_launch_description(): ]) transport_1 = Node( - package='task_planner', + package='robossembler', executable='move_action_node', name='transport_1', namespace=namespace, @@ -99,6 +100,7 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(declare_namespace_cmd) + ld.add_action(declare_robot_description) # Declare the launch options ld.add_action(plansys2_cmd) diff --git a/robossembler/src/move_controller_node.cpp b/robossembler/src/move_controller_node.cpp index a506d58..fcfbe6a 100644 --- a/robossembler/src/move_controller_node.cpp +++ b/robossembler/src/move_controller_node.cpp @@ -37,7 +37,7 @@ public: void init_knowledge() { - problem_expert_->addInstance(plansys2::Instance{"rasms", "robot"}); + problem_expert_->addInstance(plansys2::Instance{"rasmt", "robot"}); problem_expert_->addInstance(plansys2::Instance{"one", "zone"}); } @@ -68,7 +68,7 @@ public: void step() { - problem_expert_->setGoal(plansys2::Goal("(and(robot_move rasms one))")); + problem_expert_->setGoal(plansys2::Goal("(and(robot_move rasmt one))")); auto domain = domain_expert_->getDomain(); auto problem = problem_expert_->getProblem();