search

AndrejOrsula / pymoveit2

Basic Python interface for MoveIt 2 built on top of ROS 2 actions and services
BSD 3-Clause "New" or "Revised" License
151 stars 56 forks source link

Kinova Gen 3 Lite #78

Closed htchr closed 2 weeks ago

htchr commented 2 weeks ago

Hello,

I'm attempting to modify the ex_pose_goal.py example to work with a Kinova Gen 3 Lite robot arm in ros2 Humble. So far, the script appears to run fine, but the robot arm doesn't move. Below are the modified script and terminal outputs:

(modified) ex_pose_goal.py

#!/usr/bin/env python3
"""
Example of moving to a pose goal.
- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=1.0
- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=0.0
"""

from threading import Thread

import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node

from pymoveit2 import MoveIt2, MoveIt2State

def main():
    rclpy.init()

    # Create node for this example
    node = Node("ex_pose_goal")

    # Declare parameters for position and orientation
    node.declare_parameter("position", [-0.018, -0.261, 0.629])
    node.declare_parameter("quat_xyzw", [0.238, 0.534, 0.373, 0.720])
    node.declare_parameter("synchronous", True)
    # If non-positive, don't cancel. Only used if synchronous is False
    node.declare_parameter("cancel_after_secs", 0.0)
    # Planner ID
    node.declare_parameter("planner_id", "RRTConnectkConfigDefault")
    # Declare parameters for cartesian planning
    node.declare_parameter("cartesian", True)
    node.declare_parameter("cartesian_max_step", 0.0025)
    node.declare_parameter("cartesian_fraction_threshold", 0.0)
    node.declare_parameter("cartesian_jump_threshold", 0.0)
    node.declare_parameter("cartesian_avoid_collisions", False)

    # Create callback group that allows execution of callbacks in parallel without restrictions
    callback_group = ReentrantCallbackGroup()

    moveit2 = MoveIt2(
        node=node,
        joint_names=['joint_1',
                     'joint_2',
                     'joint_3',
                     'joint_4',
                     'joint_5',
                     'joint_6',
                     'right_finger_bottom_joint'],
        base_link_name='base_link',
        end_effector_name='end_effector_link',
        group_name='gen3_lite_arm',
        callback_group=callback_group,
    )
    moveit2.planner_id = (
        node.get_parameter("planner_id").get_parameter_value().string_value
    )

    # Spin the node in background thread(s) and wait a bit for initialization
    executor = rclpy.executors.MultiThreadedExecutor(2)
    executor.add_node(node)
    executor_thread = Thread(target=executor.spin, daemon=True, args=())
    executor_thread.start()
    node.create_rate(1.0).sleep()

    # Scale down velocity and acceleration of joints (percentage of maximum)
    moveit2.max_velocity = 0.5
    moveit2.max_acceleration = 0.5

    # Get parameters
    position = node.get_parameter("position").get_parameter_value().double_array_value
    quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
    synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value
    cancel_after_secs = (
        node.get_parameter("cancel_after_secs").get_parameter_value().double_value
    )
    cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value
    cartesian_max_step = (
        node.get_parameter("cartesian_max_step").get_parameter_value().double_value
    )
    cartesian_fraction_threshold = (
        node.get_parameter("cartesian_fraction_threshold")
        .get_parameter_value()
        .double_value
    )
    cartesian_jump_threshold = (
        node.get_parameter("cartesian_jump_threshold")
        .get_parameter_value()
        .double_value
    )
    cartesian_avoid_collisions = (
        node.get_parameter("cartesian_avoid_collisions")
        .get_parameter_value()
        .bool_value
    )

    # Set parameters for cartesian planning
    moveit2.cartesian_avoid_collisions = cartesian_avoid_collisions
    moveit2.cartesian_jump_threshold = cartesian_jump_threshold

    # Move to pose
    node.get_logger().info(
        f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
    )
    moveit2.move_to_pose(
        position=position,
        quat_xyzw=quat_xyzw,
        cartesian=cartesian,
        cartesian_max_step=cartesian_max_step,
        cartesian_fraction_threshold=cartesian_fraction_threshold,
    )
    if synchronous:
        # Note: the same functionality can be achieved by setting
        # `synchronous:=false` and `cancel_after_secs` to a negative value.
        moveit2.wait_until_executed()
    else:
        # Wait for the request to get accepted (i.e., for execution to start)
        print("Current State: " + str(moveit2.query_state()))
        rate = node.create_rate(10)
        while moveit2.query_state() != MoveIt2State.EXECUTING:
            rate.sleep()

        # Get the future
        print("Current State: " + str(moveit2.query_state()))
        future = moveit2.get_execution_future()

        # Cancel the goal
        if cancel_after_secs > 0.0:
            # Sleep for the specified time
            sleep_time = node.create_rate(cancel_after_secs)
            sleep_time.sleep()
            # Cancel the goal
            print("Cancelling goal")
            moveit2.cancel_execution()

        # Wait until the future is done
        while not future.done():
            rate.sleep()

        # Print the result
        print("Result status: " + str(future.result().status))
        print("Result error code: " + str(future.result().result.error_code))

    rclpy.shutdown()
    executor_thread.join()
    exit(0)

if __name__ == "__main__":
    main()

Terminal output of Kinova Gazebo:

vscode@legion24:/kinova_ws$ ros2 launch kortex_bringup kortex_sim_control.launch.py sim_ignition:=false sim_gazebo:=true robot_type:=gen3_lite robot_name:=gen3_lite dof:=6 gripper:=gen3_lite_2f launch_rviz:=false
[INFO] [launch]: All log files can be found below /home/vscode/.ros/log/2024-10-23-15-20-57-715235-legion24-375177
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [parameter_bridge-1]: process started with pid [375180]
[INFO] [robot_state_publisher-2]: process started with pid [375182]
[INFO] [spawner-3]: process started with pid [375184]
[INFO] [spawner-4]: process started with pid [375186]
[INFO] [spawner-5]: process started with pid [375188]
[INFO] [gzserver-6]: process started with pid [375190]
[INFO] [gzclient-7]: process started with pid [375192]
[INFO] [spawn_entity.py-8]: process started with pid [375194]
[INFO] [parameter_bridge-9]: process started with pid [375196]
[robot_state_publisher-2] [INFO] [1729722057.899276321] [robot_state_publisher]: got segment arm_link
[robot_state_publisher-2] [INFO] [1729722057.899346296] [robot_state_publisher]: got segment base_link
[robot_state_publisher-2] [INFO] [1729722057.899350006] [robot_state_publisher]: got segment dummy_link
[robot_state_publisher-2] [INFO] [1729722057.899352029] [robot_state_publisher]: got segment end_effector_link
[robot_state_publisher-2] [INFO] [1729722057.899354102] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-2] [INFO] [1729722057.899356151] [robot_state_publisher]: got segment gripper_base_link
[robot_state_publisher-2] [INFO] [1729722057.899358172] [robot_state_publisher]: got segment left_finger_dist_link
[robot_state_publisher-2] [INFO] [1729722057.899360589] [robot_state_publisher]: got segment left_finger_prox_link
[robot_state_publisher-2] [INFO] [1729722057.899362518] [robot_state_publisher]: got segment lower_wrist_link
[robot_state_publisher-2] [INFO] [1729722057.899364460] [robot_state_publisher]: got segment right_finger_dist_link
[robot_state_publisher-2] [INFO] [1729722057.899366304] [robot_state_publisher]: got segment right_finger_prox_link
[robot_state_publisher-2] [INFO] [1729722057.899368258] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-2] [INFO] [1729722057.899370221] [robot_state_publisher]: got segment tool_frame
[robot_state_publisher-2] [INFO] [1729722057.899372156] [robot_state_publisher]: got segment upper_wrist_link
[robot_state_publisher-2] [INFO] [1729722057.899373975] [robot_state_publisher]: got segment world
[parameter_bridge-1] [INFO] [1729722057.906341995] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/clock (ignition.msgs.Clock) -> /clock (rosgraph_msgs/msg/Clock)] (Lazy 0)
[parameter_bridge-9] [INFO] [1729722057.907040501] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/wrist_mounted_camera/image (ignition.msgs.Image) -> /wrist_mounted_camera/image (sensor_msgs/msg/Image)] (Lazy 0)
[parameter_bridge-9] [INFO] [1729722057.907958334] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/wrist_mounted_camera/depth_image (ignition.msgs.Image) -> /wrist_mounted_camera/depth_image (sensor_msgs/msg/Image)] (Lazy 0)
[parameter_bridge-9] [INFO] [1729722057.908123145] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/wrist_mounted_camera/points (ignition.msgs.PointCloudPacked) -> /wrist_mounted_camera/points (sensor_msgs/msg/PointCloud2)] (Lazy 0)
[parameter_bridge-9] [INFO] [1729722057.908376780] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/wrist_mounted_camera/camera_info (ignition.msgs.CameraInfo) -> /wrist_mounted_camera/camera_info (sensor_msgs/msg/CameraInfo)] (Lazy 0)
[spawner-5] [INFO] [1729722058.019578905] [spawner_twist_controller]: waiting for service /controller_manager/list_controllers to become available...
[spawner-3] [INFO] [1729722058.023441861] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[spawner-4] [INFO] [1729722058.029566217] [spawner_joint_trajectory_controller]: waiting for service /controller_manager/list_controllers to become available...
[spawn_entity.py-8] [INFO] [1729722058.076533365] [spawn_robot]: Spawn Entity started
[spawn_entity.py-8] [INFO] [1729722058.076682268] [spawn_robot]: Loading entity published on topic robot_description
[spawn_entity.py-8] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[spawn_entity.py-8]   warnings.warn(
[spawn_entity.py-8] [INFO] [1729722058.077560903] [spawn_robot]: Waiting for entity xml on robot_description
[spawn_entity.py-8] [INFO] [1729722058.088451288] [spawn_robot]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-8] [INFO] [1729722058.088582247] [spawn_robot]: Waiting for service /spawn_entity
[gzserver-6] ALSA lib pcm_dmix.c:1032:(snd_pcm_dmix_open) unable to open slave
[gzserver-6] AL lib: (EE) ALCplaybackAlsa_open: Could not open playback device 'default': No such file or directory
[spawn_entity.py-8] [INFO] [1729722058.590870070] [spawn_robot]: Calling service /spawn_entity
[spawn_entity.py-8] [INFO] [1729722058.814659649] [spawn_robot]: Spawn status: SpawnEntity: Successfully spawned entity [gen3_lite]
[INFO] [spawn_entity.py-8]: process has finished cleanly [pid 375194]
[gzserver-6] [INFO] [1729722058.942605202] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-6] [INFO] [1729722058.946000298] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-6] [INFO] [1729722058.946118600] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-6] [INFO] [1729722058.947371884] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-6] [INFO] [1729722058.947655801] [gazebo_ros2_control]: Received urdf from param server, parsing...
[gzserver-6] [INFO] [1729722058.947678805] [gazebo_ros2_control]: Loading parameter files /kinova_ws/install/share/kortex_description/arms/gen3_lite/6dof/config/ros2_controllers.yaml
[gzserver-6] [INFO] [1729722058.949671121] [resource_manager]: Loading hardware 'IgnitionSystem' 
[gzserver-6] [ERROR] [1729722058.949782422] [gazebo_ros2_control]: Error initializing URDF to resource manager!
[gzserver-6] [INFO] [1729722058.953963509] [gazebo_ros2_control]: Loading joint: joint_1
[gzserver-6] [INFO] [1729722058.953982039] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.953990074] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.953998017] [gazebo_ros2_control]:                        found initial value: 0.000000
[gzserver-6] [INFO] [1729722058.954002874] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954006526] [gazebo_ros2_control]:                effort
[gzserver-6] [INFO] [1729722058.954011619] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954013910] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954098326] [gazebo_ros2_control]: Loading joint: joint_2
[gzserver-6] [INFO] [1729722058.954101973] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.954104513] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954106817] [gazebo_ros2_control]:                        found initial value: 0.000000
[gzserver-6] [INFO] [1729722058.954109470] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954111843] [gazebo_ros2_control]:                effort
[gzserver-6] [INFO] [1729722058.954113993] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954116019] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954141438] [gazebo_ros2_control]: Loading joint: joint_3
[gzserver-6] [INFO] [1729722058.954144172] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.954146533] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954148798] [gazebo_ros2_control]:                        found initial value: 0.000000
[gzserver-6] [INFO] [1729722058.954151488] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954153891] [gazebo_ros2_control]:                effort
[gzserver-6] [INFO] [1729722058.954156755] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954159188] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954235888] [gazebo_ros2_control]: Loading joint: joint_4
[gzserver-6] [INFO] [1729722058.954238992] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.954241469] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954243722] [gazebo_ros2_control]:                        found initial value: 0.000000
[gzserver-6] [INFO] [1729722058.954246406] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954248753] [gazebo_ros2_control]:                effort
[gzserver-6] [INFO] [1729722058.954251080] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954253649] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954276081] [gazebo_ros2_control]: Loading joint: joint_5
[gzserver-6] [INFO] [1729722058.954278650] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.954280902] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954282990] [gazebo_ros2_control]:                        found initial value: 0.000000
[gzserver-6] [INFO] [1729722058.954285457] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954287586] [gazebo_ros2_control]:                effort
[gzserver-6] [INFO] [1729722058.954290265] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954292509] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954312954] [gazebo_ros2_control]: Loading joint: joint_6
[gzserver-6] [INFO] [1729722058.954315456] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.954317809] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954319921] [gazebo_ros2_control]:                        found initial value: 0.000000
[gzserver-6] [INFO] [1729722058.954322577] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954329576] [gazebo_ros2_control]:                effort
[gzserver-6] [INFO] [1729722058.954332224] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954346616] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954401979] [gazebo_ros2_control]: Loading joint: right_finger_bottom_joint
[gzserver-6] [INFO] [1729722058.954405443] [gazebo_ros2_control]:       State:
[gzserver-6] [INFO] [1729722058.954408216] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954410902] [gazebo_ros2_control]:                velocity
[gzserver-6] [INFO] [1729722058.954413528] [gazebo_ros2_control]:       Command:
[gzserver-6] [INFO] [1729722058.954420556] [gazebo_ros2_control]:                position
[gzserver-6] [INFO] [1729722058.954477386] [resource_manager]: Initialize hardware 'IgnitionSystem' 
[gzserver-6] [INFO] [1729722058.954552817] [resource_manager]: Successful initialization of hardware 'IgnitionSystem'
[gzserver-6] [INFO] [1729722058.954633909] [resource_manager]: 'configure' hardware 'IgnitionSystem' 
[gzserver-6] [INFO] [1729722058.954638304] [resource_manager]: Successful 'configure' of hardware 'IgnitionSystem'
[gzserver-6] [INFO] [1729722058.954644156] [resource_manager]: 'activate' hardware 'IgnitionSystem' 
[gzserver-6] [INFO] [1729722058.954646500] [resource_manager]: Successful 'activate' of hardware 'IgnitionSystem'
[gzserver-6] [INFO] [1729722058.954679334] [gazebo_ros2_control]: Loading controller_manager
[gzserver-6] [INFO] [1729722058.962025335] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-6] [INFO] [1729722059.103308067] [controller_manager]: Loading controller 'twist_controller'
[gzserver-6] [INFO] [1729722059.111770988] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-5] [INFO] [1729722059.112281915] [spawner_twist_controller]: Loaded twist_controller
[gzserver-6] [INFO] [1729722059.117653384] [controller_manager]: Loading controller 'joint_trajectory_controller'
[spawner-3] [INFO] [1729722059.118141468] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[gzserver-6] [WARN] [1729722059.127263880] [joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[gzserver-6] [INFO] [1729722059.128028335] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[gzserver-6] [INFO] [1729722059.128154656] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-4] [INFO] [1729722059.128355216] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller
[gzserver-6] [INFO] [1729722059.132951990] [controller_manager]: Configuring controller 'joint_trajectory_controller'
[gzserver-6] [INFO] [1729722059.133048058] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[gzserver-6] [INFO] [1729722059.133071910] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[gzserver-6] [INFO] [1729722059.133103718] [joint_trajectory_controller]: Using 'splines' interpolation method.
[gzserver-6] [INFO] [1729722059.133783064] [joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[gzserver-6] [INFO] [1729722059.134455105] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-3] [INFO] [1729722059.139413520] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[spawner-4] [INFO] [1729722059.141479020] [spawner_joint_trajectory_controller]: Configured and activated joint_trajectory_controller
[gzserver-6] [INFO] [1729722059.193613682] [controller_manager]: Configuring controller 'twist_controller'
[gzserver-6] [INFO] [1729722059.194295808] [twist_controller]: configure successful
[INFO] [spawner-3]: process has finished cleanly [pid 375184]
[INFO] [spawner-4]: process has finished cleanly [pid 375186]
[INFO] [spawner-5]: process has finished cleanly [pid 375188]
[gzserver-6] [INFO] [1729722075.493045052] [joint_trajectory_controller]: Received new action goal
[gzserver-6] [INFO] [1729722075.493100723] [joint_trajectory_controller]: Accepted new action goal
[gzserver-6] [INFO] [1729722075.494063624] [joint_trajectory_controller]: Goal reached, success!

Terminal output of MoveIt2:

vscode@legion24:/kinova_ws$ ros2 launch kinova_gen3_lite_moveit_config sim.launch.py 
[INFO] [launch]: All log files can be found below /home/vscode/.ros/log/2024-10-23-15-21-05-308824-legion24-375766
[INFO] [launch]: Default logging verbosity is set to INFO
Using load_yaml() directly is deprecated. Use xacro.load_yaml() instead.
[INFO] [move_group-1]: process started with pid [375768]
[INFO] [rviz2-2]: process started with pid [375770]
[rviz2-2] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-vscode'
[rviz2-2] [INFO] [1729722065.756092303] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1729722065.756200159] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-2] [INFO] [1729722065.772888334] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-2]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[rviz2-2] [ERROR] [1729722068.819627818] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-2] [INFO] [1729722068.830486193] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-2] [INFO] [1729722068.882718352] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[rviz2-2] [INFO] [1729722068.882745409] [moveit_robot_model.robot_model]: Loading robot model 'gen3_lite'...
[rviz2-2] [INFO] [1729722068.882751615] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-2] [WARN] [1729722068.990723441] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'gen3_lite_gripper'
[rviz2-2] [INFO] [1729722069.347833383] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-2] [INFO] [1729722069.348285102] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-2] [INFO] [1729722069.494123105] [interactive_marker_display_99646268849504]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-2] [INFO] [1729722069.514200495] [interactive_marker_display_99646268849504]: Sending request for interactive markers
[rviz2-2] [INFO] [1729722069.547355008] [interactive_marker_display_99646268849504]: Service response received for initialization
[rviz2-2] [INFO] [1729722069.581988522] [moveit_ros_visualization.motion_planning_frame]: group gen3_lite_arm
[rviz2-2] [INFO] [1729722069.582002730] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'gen3_lite_arm' in namespace ''
[rviz2-2] [INFO] [1729722069.585610406] [move_group_interface]: Ready to take commands for planning group gen3_lite_arm.
[rviz2-2] [INFO] [1729722069.586144628] [moveit_ros_visualization.motion_planning_frame]: group gen3_lite_arm

Terminal output of pymovit2:

vscode@legion24:/kinova_ws$ ros2 run t516_lab7 ex_pose_goal 
[INFO] [1729722075.391154304] [ex_pose_goal]: Moving to {position: [-0.018, -0.261, 0.629], quat_xyzw: [0.238, 0.534, 0.373, 0.72]}

Thank you,

htchr commented 2 weeks ago

When the "synchronous" parameter is set to False, the terminal output reports that MoveIt2State is "REQUESTING" and hangs forever, regardless is the "cancel_after_secs" parameter is positive or negative:

vscode@legion24:/kinova_ws$ ros2 run t516_lab7 ex_pose_goal 
[INFO] [1729801459.749133651] [ex_pose_goal]: Moving to {position: [-0.018, -0.261, 0.629], quat_xyzw: [0.238, 0.534, 0.373, 0.72]}
Current State: MoveIt2State.REQUESTING
htchr commented 2 weeks ago

I have found a solution:

I use this command to launch the Kinova arm in simulation:

ros2 launch kortex_bringup kortex_sim_control.launch.py sim_ignition:=false sim_gazebo:=true robot_type:=gen3_lite robot_name:=gen3_lite dof:=6 gripper:=gen3_lite_2f use_sim_time:=true launch_rviz:=false

I use this launch file instead to run MoveIt2 with the Kinova arm:

ros2 launch kinova_gen3_lite_moveit_config move_group.launch.py use_sim_time:=true

Even though both simulation files set use_sim_time:=true, I have to manually reconfigure move_group and moveit_simple_controller_manager to use simulation time in rqt.