gtirth commented 1 year ago

Hello,

I am trying to use kortex_moveit for multiple gen3 arm but running into an issue when i try to launch it.

Can someone please suggest a solution for this.

moriarty commented 1 year ago

This seems familiar but I'm not on my computer right now.

Which version of MoveIt are you using?

Can you provide your launch file?

gtirth commented 1 year ago

launch_file.txt

I am using moveit_2 for humble distro.

I have attached file that i created for custom robot that has 2 gen3 arms. There might be some more changes which i would have to do but i was not able to launch it so i did not do all the modifications in the launch file. I was able to create the move_it package with moveit_setup_assistant but I have not been able to create a launch file that would allow me move the arms. I was able to test and move individual arms with kinova_gen3_7dof_robotiq_2f_85_moveit_config.

I would appreciate if you help or redirect me to some resources which would allow to do that

gtirth commented 1 year ago

Hello,

I am using moveit_2 for humble distro. I have attached file that i created for custom robot that has 2 gen3 arms. There might be some more changes which i would have to do but i was not able to launch it so i did not do all the modifications in the launch file. I was able to create the move_it package with moveit_setup_assistant but I have not been able to create a launch file that would allow me move the arms. I was able to test and move individual arms with kinova_gen3_7dof_robotiq_2f_85_moveit_config.

I would appreciate if you help or redirect me to some resources which would allow to do that

[ From: Alex Moriarty @.> Sent: 13 September 2023 18:30 To: Kinovarobotics/ros2_kortex @.> Cc: Tirth Gajera @.>; Author @.> Subject: Re: [Kinovarobotics/ros2_kortex] kortex_moveit.launch.py (Issue #172)

This seems familiar but I'm not on my computer right now.

Which version of MoveIt are you using?

Can you provide your launch file?

— Reply to this email directly, view it on GitHubhttps://github.com/Kinovarobotics/ros2_kortex/issues/172#issuecomment-1718397658, or unsubscribehttps://github.com/notifications/unsubscribe-auth/A2DBMOSOT5VBMLJQXDCQII3X2IXZRANCNFSM6AAAAAA4WXTHR4. You are receiving this because you authored the thread.Message ID: @.***>

Caution: This email originated from outside of the organization. Please take care when clicking links or opening attachments, and never send money to anyone through an email or reply to an email asking to send money, it is a scam. When in doubt, contact your IT Department

Copyright (c) 2021 PickNik, Inc.

#

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.

#

Author: Denis Stogl

import os

import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import ( Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare

def load_yaml(package_name, file_path): package_path = get_package_share_directory(package_name) absolute_file_path = os.path.join(package_path, file_path)

try:
    with open(absolute_file_path) as file:
        return yaml.safe_load(file)
except OSError:  # parent of IOError, OSError *and* WindowsError where available
    return None

def generate_launch_description(): declared_arguments = []

Robot specific arguments

declared_arguments.append(
    DeclareLaunchArgument(
        "robot_type", description="Type/series of robot.", choices=["gen3", "gen3_lite"], default_value="gen3"
    )
)
declared_arguments.append(DeclareLaunchArgument("dof", description="DoF of robot."))
declared_arguments.append(
    DeclareLaunchArgument(
        "robot_ip", description="IP address by which the robot can be reached.", default_value= "192.168.42.191"
    )
)
# General arguments
declared_arguments.append(
    DeclareLaunchArgument(
        "description_package",
        default_value="rosie_description",
        description="Description package with robot URDF/XACRO files. Usually the argument \
    is not set, it enables use of a custom description.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "description_file",
        default_value="rosie.urdf.xacro",
        description="URDF/XACRO description file with the robot.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "moveit_config_package",
        default_value="rosie_move_it",
        description="MoveIt configuration package for the robot. Usually the argument \
    is not set, it enables use of a custom config package.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "moveit_config_file",
        default_value="rosie.srdf.xacro",
        description="MoveIt SRDF/XACRO description file with the robot.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "robot_name",
        default_value="arm",
        description="Name of the robot.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "prefix",
        default_value='"left"',
        description="Prefix of the joint names, useful for \
    multi-robot setup. If changed than also joint names in the controllers' configuration \
    have to be updated.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "gripper",
        default_value='"robotiq_2f_85"',
        description="Name of the gripper attached to the arm",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "use_fake_hardware",
        default_value="false",
        description="Start robot with fake hardware mirroring command to its states.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument(
        "fake_sensor_commands",
        default_value="false",
        description="Enable fake command interfaces for sensors used for simple simulations. \
        Used only if 'use_fake_hardware' parameter is true.",
    )
)
declared_arguments.append(
    DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)

# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
robot_ip = LaunchConfiguration("robot_ip")
dof = LaunchConfiguration("dof")
# General arguments
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
robot_name = LaunchConfiguration("robot_name")
prefix = LaunchConfiguration("prefix")
gripper = LaunchConfiguration("gripper")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
launch_rviz = LaunchConfiguration("launch_rviz")

robot_description_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        PathJoinSubstitution(
            [FindPackageShare(description_package), "urdf", description_file]
        ),
        " ",
        "robot_ip:=",
        robot_ip,
        " ",
        "name:=",
        robot_name,
        " ",
        "arm:=",
        robot_type,
        " ",
        "dof:=",
        dof,
        " ",
        "prefix:=",
        prefix,
        " ",
        "use_fake_hardware:=",
        use_fake_hardware,
        " ",
        "fake_sensor_commands:=",
        fake_sensor_commands,
        " ",
        "gripper:=",
        gripper,
        " ",
    ]
)
robot_description = {"robot_description": robot_description_content}

# MoveIt Configuration
robot_description_semantic_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        PathJoinSubstitution(
            [
                FindPackageShare(moveit_config_package),
                "config",
                #dof + "dof",
                moveit_config_file,
            ]
        ),
        " ",
        "name:=",
        robot_name,
        " ",
        "prefix:=",
        prefix,
        " ",
    ]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}

kinematics_yaml = load_yaml(moveit_config_package, "config/kinematics.yaml")
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}

# Planning Configuration
ompl_planning_pipeline_config = {
    "move_group": {
        "planning_plugin": "ompl_interface/OMPLPlanner",
        "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
        "start_state_max_bounds_error": 0.1,
    }
}

ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)

# Trajectory Execution Configuration
# TODO(destogl): check how to use ros2_control's controllers-yaml
controllers_yaml = load_yaml(moveit_config_package, "config/" + dof + "dof/controllers.yaml")
moveit_controllers = {
    "moveit_simple_controller_manager": controllers_yaml,
    "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}

trajectory_execution = {
    "moveit_manage_controllers": False,
    "trajectory_execution.allowed_execution_duration_scaling": 1.2,
    "trajectory_execution.allowed_goal_duration_margin": 0.5,
    "trajectory_execution.allowed_start_tolerance": 0.01,
}

planning_scene_monitor_parameters = {
    "publish_planning_scene": True,
    "publish_geometry_updates": True,
    "publish_state_updates": True,
    "publish_transforms_updates": True,
    "planning_scene_monitor_options": {
        "name": "planning_scene_monitor",
        "robot_description": "robot_description",
        "joint_state_topic": "/joint_states",
        "attached_collision_object_topic": "/move_group/planning_scene_monitor",
        "publish_planning_scene_topic": "/move_group/publish_planning_scene",
        "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
        "wait_for_initial_state_timeout": 10.0,
    },
}

# Start the actual move_group node/action server
move_group_node = Node(
    package="moveit_ros_move_group",
    executable="move_group",
    output="screen",
    parameters=[
        robot_description,
        robot_description_semantic,
        robot_description_kinematics,
        ompl_planning_pipeline_config,
        trajectory_execution,
        moveit_controllers,
        planning_scene_monitor_parameters,
    ],
)

# rviz with moveit configuration
rviz_config_file = PathJoinSubstitution(
    [FindPackageShare(moveit_config_package), "rviz", "moveit.rviz"]
)
rviz_node = Node(
    package="rviz2",
    condition=IfCondition(launch_rviz),
    executable="rviz2",
    name="rviz2_moveit",
    output="log",
    arguments=["-d", rviz_config_file],
    parameters=[
        robot_description,
        robot_description_semantic,
        ompl_planning_pipeline_config,
        robot_description_kinematics,
    ],
)

nodes_to_start = [
    move_group_node,
    rviz_node,
]

return LaunchDescription(declared_arguments + nodes_to_start)

jalapatip commented 10 months ago

@gtirth, I'm working on a multiarm setup too but haven't had much luck. I was wondering if you were able to set it up.

Kinovarobotics / ros2_kortex

kortex_moveit.launch.py #172

Copyright (c) 2021 PickNik, Inc.

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.

Author: Denis Stogl

Robot specific arguments