search

Kinovarobotics / kinova-ros

ROS packages for Jaco2 and Mico robotic arms
BSD 3-Clause "New" or "Revised" License
363 stars 318 forks source link

m1n6s300_moveit_config demo launch fails to execute #441

Open GarmischWg opened 1 month ago

GarmischWg commented 1 month ago

Description

I am trying to use moveit to control the robotic arm m1n6s300, but i encoubnter issues with move_group controllers. Through the kinova_interactive_control node, have confirmed that the robotic arm can be control through the computer, but cannot be controlled through moveit.

Version

ROS distribution : Noetic Branch and commit you are using : Noetic devel

Steps to reproduce

  1. following instructions from the Moveit Section
  2. Running the folowing 2 commands in 2 seperate terminal 
    roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300
roslaunch m1n6s300_moveit_config m1n6s300_demo.launch
  3. Then Rviz will start, move the end effector to a different position
  4. Press plan and execute
  5. Execution will then fail

Expected behavior

The rviz will be able to plan the trajectory and the robotic arm would execute it.

Error logs/Terminal output

# Terminal 1
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=m1n6s300

process[m1n6s300_driver-2]: started with pid [73955]
process[m1n6s300_state_publisher-3]: started with pid [73956]
[ INFO] [1720782021.117304492]: kinova_robotType is m1n6s300.
[ INFO] [1720782021.118113907]: kinova_robotName is m1n6s300.
[ INFO] [1720782021.120667506]: Initializing Kinova USB API (header version: 50300, library version: 5.2.0)
[ INFO] [1720782021.279917131]: Found 1 device(s), using device at index 0 (model: Mico Service, serial number: PJ-----------0 , code version: 393481, code revision: 6)
[ INFO] [1720782021.352973166]: Initializing fingers...this will take a few seconds and the fingers should open completely
[ INFO] [1720782021.441604662]: The arm is ready to use.
m1n6s300_joint_1 m1n6s300_joint_2 m1n6s300_joint_3 m1n6s300_joint_4 m1n6s300_joint_5 m1n6s300_joint_6

# Terminal 2
roslaunch m1n6s300_moveit_config m1n6s300_demo.launch

# some warnings and errors from terminal 2
ROS_MASTER_URI=http://localhost:11311

process[m1n6s300_joint_trajectory_action_server-1]: started with pid [75590]
process[m1n6s300_gripper_command_action_server-2]: started with pid [75591]
process[move_group-3]: started with pid [75592]
process[rviz_isd_ROG_Strix_SCAR_18_G834JYR_G834JYR_75512_4786984848253227277-4]: started with pid [75594]
[ WARN] [1720782681.456862727]: MoveGroup launched with ~default_planning_pipeline '' not configured in ~planning_pipelines
[ WARN] [1720782681.457127337]: Falling back to using the move_group node's namespace (deprecated Melodic behavior).
[ INFO] [1720782681.459723164]: Loading robot model 'm1n6s300'...
[ INFO] [1720782681.513197696]: rviz version 1.14.25
[ INFO] [1720782681.513218894]: compiled against Qt version 5.12.8
[ INFO] [1720782681.513222431]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1720782681.519241128]: Forcing OpenGl version 0.
[ WARN] [1720782681.553433700]: Kinematics solver doesn't support #attempts anymore, but only a timeout. Please remove the parameter '/robot_description_kinematics/arm/kinematics_solver_attempts' from your configuration.
[ WARN] [1720782681.556284656]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ INFO] [1720782681.558135578]: IK Using joint m1n6s300_link_1 -3.40282e+38 3.40282e+38

# Skipping some lines

[ INFO] [1720782685.127784705]: Starting planning scene monitor
[ INFO] [1720782685.130003892]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1720782685.218859448]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ WARN] [1720782686.855563100]: Waiting for m1n6s300/effort_joint_trajectory_controller/follow_joint_trajectory to come up...
[ INFO] [1720782690.237495682]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
[ INFO] [1720782690.237618241]: Constructing new MoveGroup connection for group 'arm' in namespace ''
[ WARN] [1720782691.855699443]: Waiting for m1n6s300/effort_joint_trajectory_controller/follow_joint_trajectory to come up...
[ERROR] [1720782696.855878052]: Action client not connected: m1n6s300/effort_joint_trajectory_controller/follow_joint_trajectory
[ WARN] [1720782701.879202387]: Waiting for m1n6s300/effort_finger_trajectory_controller/follow_joint_trajectory to come up...
[ WARN] [1720782706.879325485]: Waiting for m1n6s300/effort_finger_trajectory_controller/follow_joint_trajectory to come up...
[ERROR] [1720782711.879478855]: Action client not connected: m1n6s300/effort_finger_trajectory_controller/follow_joint_trajectory
[ INFO] [1720782711.896405377]: Returned 0 controllers in list
[ INFO] [1720782711.903937686]: Trajectory execution is managing controllers
[ INFO] [1720782711.903964092]: MoveGroup debug mode is ON

# I attempt to execute a trajectory
[ERROR] [1720785071.936976136]: Unable to identify any set of controllers that can actuate the specified joints: [ m1n6s300_joint_1 m1n6s300_joint_2 m1n6s300_joint_3 m1n6s300_joint_4 m1n6s300_joint_5 m1n6s300_joint_6 ]
[ERROR] [1720785071.937001128]: Known controllers and their joints:

[ INFO] [1720785071.975678069]: ABORTED: CONTROL_FAILED

In moveit, planning for a trajectory would work, but when i attempt to execute the path, it would not work. In an attempt to debug the problem, i ran the rostopic list command:


rostopic list
/attached_collision_object
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/m1n6s300/follow_joint_trajectory/cancel
/m1n6s300/follow_joint_trajectory/feedback
/m1n6s300/follow_joint_trajectory/goal
/m1n6s300/follow_joint_trajectory/result
/m1n6s300/follow_joint_trajectory/status
/m1n6s300_driver/fingers_action/finger_positions/cancel
/m1n6s300_driver/fingers_action/finger_positions/feedback
/m1n6s300_driver/fingers_action/finger_positions/goal
/m1n6s300_driver/fingers_action/finger_positions/result
/m1n6s300_driver/fingers_action/finger_positions/status
/m1n6s300_driver/in/cartesian_force
/m1n6s300_driver/in/cartesian_velocity
/m1n6s300_driver/in/cartesian_velocity_with_finger_velocity
/m1n6s300_driver/in/cartesian_velocity_with_fingers
/m1n6s300_driver/in/joint_torque
/m1n6s300_driver/in/joint_velocity
/m1n6s300_driver/joints_action/joint_angles/cancel
/m1n6s300_driver/joints_action/joint_angles/feedback
/m1n6s300_driver/joints_action/joint_angles/goal
/m1n6s300_driver/joints_action/joint_angles/result
/m1n6s300_driver/joints_action/joint_angles/status
/m1n6s300_driver/out/cartesian_command
/m1n6s300_driver/out/finger_position
/m1n6s300_driver/out/joint_angles
/m1n6s300_driver/out/joint_command
/m1n6s300_driver/out/joint_state
/m1n6s300_driver/out/joint_torques
/m1n6s300_driver/out/tool_pose
/m1n6s300_driver/out/tool_wrench
/m1n6s300_driver/pose_action/tool_pose/cancel
/m1n6s300_driver/pose_action/tool_pose/feedback
/m1n6s300_driver/pose_action/tool_pose/goal
/m1n6s300_driver/pose_action/tool_pose/result
/m1n6s300_driver/pose_action/tool_pose/status
/m1n6s300_driver/trajectory_controller/command
/m1n6s300_driver/trajectory_controller/state
/m1n6s300_gripper/gripper_command/cancel
/m1n6s300_gripper/gripper_command/feedback
/m1n6s300_gripper/gripper_command/goal
/m1n6s300_gripper/gripper_command/result
/m1n6s300_gripper/gripper_command/status
/move_group/cancel
/move_group/display_contacts
/move_group/display_cost_sources
/move_group/display_grasp_markers
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/motion_plan_request
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/recognized_object_array
/rosout
/rosout_agg
/rviz_isd_ROG_Strix_SCAR_18_G834JYR_G834JYR_76586_7502612256434255150/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_isd_ROG_Strix_SCAR_18_G834JYR_G834JYR_76586_7502612256434255150/motionplanning_planning_scene_monitor/parameter_updates
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/tf
/tf_static
/trajectory_execution_event

I can see that the closest thing to the missing "m1n6s300/effort_finger_trajectory_controller/follow_joint_trajectory" topic is "/m1n6s300/follow_joint_trajectory", therefore, in kinova_moveit/robot_configs/m1n6s300_moveit_config/config/controllers_ros_control.yaml, i modified name: m1n6s300/effort_finger_trajectory_controller -> name: m1n6s300

After doing such modifcation and relaunching moveit, the previous errors regarding "Action client not connected" is gone, but when i again try to execute a trajectory, i still get the following error:

[ INFO] [1720784301.340700986]: Returned 1 controllers in list
[ERROR] [1720784301.340721584]: Unable to identify any set of controllers that can actuate the specified joints: [ m1n6s300_joint_1 m1n6s300_joint_2 m1n6s300_joint_3 m1n6s300_joint_4 m1n6s300_joint_5 m1n6s300_joint_6 ]
[ERROR] [1720784301.340733187]: Known controllers and their joints:
controller 'm1n6s300' controls joints:
  m1n6s300_joint_finger_1
  m1n6s300_joint_finger_2
  m1n6s300_joint_finger_3

[ INFO] [1720784301.377268566]: ABORTED: CONTROL_FAILED