Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SDA10F Fails when trying to execute movements with the 'arms' and 'sda10f' groups for moving the real robot #403

Closed
pablomalvido opened this issue May 4, 2021 · 6 comments
Labels
multi-group Issues related to multi-group support

Comments

@pablomalvido
Copy link

pablomalvido commented May 4, 2021

Hello,

I am working with a SDA10F robot with a FS100 controller and I am having problems for moving the ‘arms’ and ‘sda10f’ groups using MoveIt. This is the information about my system:

• OS version: Ubuntu 18.04.5
• ROS distribution: ROS Melodic
• Controller version: FS3.30.00A(US/FI)-00
• Motoman repository branch: kinetic-devel

I have followed the tutorials in http://wiki.ros.org/motoman_driver/Tutorials for installing the ROS server in the controller (first) and for commanding the robot using ROS (second tutorial). I have installed MotoROS using binaries (MotoRosFS_v192.out) and the MD5 hash is exactly the same as the one in the tutorial.

I run the command: roslaunch motoman_sda10f_moveit_config moveit_planning_execution.launch sim:=false robot_ip:=10.0.0.2 controller:=fs100 to control real the robot with MoveIt, and I get the following output:

... logging to /home/remodel/.ros/log/c7435418-acc1-11eb-9e95-dc7196973bd7/roslaunch-remodel-TAU-4445.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
Deprecated: xacro tag 'insert_block' w/o 'xacro:' xml namespace prefix (will be forbidden in Noetic)
when processing file: /home/remodel/catkin_ws/src/motoman/motoman_sda10f_support/urdf/sda10f.xacro
Use the following command to fix incorrect tag usage:
find . -iname "*.xacro" | xargs sed -i 's#<\([/]\?\)\(if\|unless\|include\|arg\|property\|macro\|insert_block\)#<\1xacro:\2#g'

started roslaunch server http://remodel-TAU:46867/

SUMMARY
========

PARAMETERS
 * /controller_joint_names: ['arm_left_joint_...
 * /mongo_wrapper_ros_remodel_TAU_4445_8992849688145291343/database_path: /home/remodel/cat...
 * /mongo_wrapper_ros_remodel_TAU_4445_8992849688145291343/overwrite: False
 * /move_group/allow_trajectory_execution: True
 * /move_group/allowed_execution_duration_scaling: 1.2
 * /move_group/allowed_goal_duration_margin: 0.5
 * /move_group/arm_left/longest_valid_segment_fraction: 0.05
 * /move_group/arm_left/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm_left/projection_evaluator: joints(arm_left_j...
 * /move_group/arm_right/longest_valid_segment_fraction: 0.05
 * /move_group/arm_right/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm_right/projection_evaluator: joints(arm_right_...
 * /move_group/arms/longest_valid_segment_fraction: 0.05
 * /move_group/arms/planner_configs: ['SBLkConfigDefau...
 * /move_group/arms/projection_evaluator: joints(arm_left_j...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'action_ns': 'j...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_range: 5.0
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_resolution: 0.025
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/sda10f/longest_valid_segment_fraction: 0.05
 * /move_group/sda10f/planner_configs: ['SBLkConfigDefau...
 * /move_group/sda10f/projection_evaluator: joints(torso_join...
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/torso/longest_valid_segment_fraction: 0.05
 * /move_group/torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/torso/projection_evaluator: joints(torso_join...
 * /robot_description: <?xml version="1....
 * /robot_description_kinematics/arm_left/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm_left/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm_left/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm_left/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/arm_right/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm_right/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm_right/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm_right/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/arms/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arms/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arms/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arms/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/sda10f/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/sda10f/kinematics_solver_attempts: 3
 * /robot_description_kinematics/sda10f/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/sda10f/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/torso/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/torso/kinematics_solver_attempts: 3
 * /robot_description_kinematics/torso/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/torso/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/arm_left_joint_1_s/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_1_s/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_1_s/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_1_s/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_left_joint_2_l/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_2_l/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_2_l/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_2_l/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_left_joint_3_e/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_3_e/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_3_e/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_3_e/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_left_joint_4_u/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_4_u/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_4_u/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_4_u/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_left_joint_5_r/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_5_r/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_5_r/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_5_r/max_velocity: 3.48
 * /robot_description_planning/joint_limits/arm_left_joint_6_b/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_6_b/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_6_b/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_6_b/max_velocity: 3.48
 * /robot_description_planning/joint_limits/arm_left_joint_7_t/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_left_joint_7_t/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_left_joint_7_t/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_left_joint_7_t/max_velocity: 6.97
 * /robot_description_planning/joint_limits/arm_right_joint_1_s/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_1_s/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_1_s/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_1_s/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_right_joint_2_l/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_2_l/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_2_l/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_2_l/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_right_joint_3_e/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_3_e/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_3_e/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_3_e/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_right_joint_4_u/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_4_u/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_4_u/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_4_u/max_velocity: 2.95
 * /robot_description_planning/joint_limits/arm_right_joint_5_r/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_5_r/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_5_r/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_5_r/max_velocity: 3.48
 * /robot_description_planning/joint_limits/arm_right_joint_6_b/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_6_b/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_6_b/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_6_b/max_velocity: 3.48
 * /robot_description_planning/joint_limits/arm_right_joint_7_t/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/arm_right_joint_7_t/has_velocity_limits: True
 * /robot_description_planning/joint_limits/arm_right_joint_7_t/max_acceleration: 0
 * /robot_description_planning/joint_limits/arm_right_joint_7_t/max_velocity: 6.97
 * /robot_description_planning/joint_limits/torso_joint_b1/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/torso_joint_b1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/torso_joint_b1/max_acceleration: 0
 * /robot_description_planning/joint_limits/torso_joint_b1/max_velocity: 2.26
 * /robot_description_planning/joint_limits/torso_joint_b2/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/torso_joint_b2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/torso_joint_b2/max_acceleration: 0
 * /robot_description_planning/joint_limits/torso_joint_b2/max_velocity: 2.26
 * /robot_description_semantic: <?xml version="1....
 * /robot_ip_address: 10.0.0.2
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_left/kinematics_solver: kdl_kinematics_pl...
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_left/kinematics_solver_attempts: 3
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_left/kinematics_solver_search_resolution: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_left/kinematics_solver_timeout: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_right/kinematics_solver: kdl_kinematics_pl...
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_right/kinematics_solver_attempts: 3
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_right/kinematics_solver_search_resolution: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/arm_right/kinematics_solver_timeout: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/arms/kinematics_solver: kdl_kinematics_pl...
 * /rviz_remodel_TAU_4445_1580162335124445778/arms/kinematics_solver_attempts: 3
 * /rviz_remodel_TAU_4445_1580162335124445778/arms/kinematics_solver_search_resolution: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/arms/kinematics_solver_timeout: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/sda10f/kinematics_solver: kdl_kinematics_pl...
 * /rviz_remodel_TAU_4445_1580162335124445778/sda10f/kinematics_solver_attempts: 3
 * /rviz_remodel_TAU_4445_1580162335124445778/sda10f/kinematics_solver_search_resolution: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/sda10f/kinematics_solver_timeout: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/torso/kinematics_solver: kdl_kinematics_pl...
 * /rviz_remodel_TAU_4445_1580162335124445778/torso/kinematics_solver_attempts: 3
 * /rviz_remodel_TAU_4445_1580162335124445778/torso/kinematics_solver_search_resolution: 0.005
 * /rviz_remodel_TAU_4445_1580162335124445778/torso/kinematics_solver_timeout: 0.005
 * /topic_list: [{'joints': ['arm...
 * /warehouse_exec: mongod
 * /warehouse_host: localhost
 * /warehouse_port: 33829

NODES
  /
    io_relay (motoman_driver/io_relay_bswap)
    joint_state (motoman_driver/robot_state_bswap)
    joint_trajectory_action (motoman_driver/motoman_driver_joint_trajectory_action)
    mongo_wrapper_ros_remodel_TAU_4445_8992849688145291343 (warehouse_ros/mongo_wrapper_ros.py)
    motion_streaming_interface (motoman_driver/motion_streaming_interface_bswap)
    move_group (moveit_ros_move_group/move_group)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz_remodel_TAU_4445_1580162335124445778 (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[joint_state-1]: started with pid [4463]
process[motion_streaming_interface-2]: started with pid [4464]
process[io_relay-3]: started with pid [4469]
process[joint_trajectory_action-4]: started with pid [4475]
[ WARN] [1620133674.799750222]: Failed to get '~port' parameter: using default (50242)
process[robot_state_publisher-5]: started with pid [4481]
process[move_group-6]: started with pid [4483]
process[rviz_remodel_TAU_4445_1580162335124445778-7]: started with pid [4488]
ERROR: cannot launch node of type [warehouse_ros/mongo_wrapper_ros.py]: Cannot locate node of type [mongo_wrapper_ros.py] in package [warehouse_ros]. Make sure file exists in package path and permission is set to executable (chmod +x)
[ WARN] [1620133674.864317270]: Tried to connect when socket already in connected state
[ INFO] [1620133674.876794866]: Loading robot model 'motoman_sda10f'...
[ INFO] [1620133674.930014113]: rviz version 1.13.15
[ INFO] [1620133674.930076388]: compiled against Qt version 5.9.5
[ INFO] [1620133674.930092475]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1620133674.935638923]: Forcing OpenGl version 0.
[ WARN] [1620133674.953990856]: Kinematics solver doesn't support #attempts anymore, but only a timeout.
Please remove the parameter '/robot_description_kinematics/arm_left/kinematics_solver_attempts' from your configuration.
[ERROR] [1620133674.989663174]: Group 'torso' is not a chain
[ERROR] [1620133674.989699157]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'torso'
[ERROR] [1620133674.989756775]: Kinematics solver could not be instantiated for joint group torso.
[ERROR] [1620133674.989822042]: Group 'arms' is not a chain
[ERROR] [1620133674.989840004]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'arms'
[ERROR] [1620133674.989853094]: Kinematics solver could not be instantiated for joint group arms.
[ERROR] [1620133674.989873725]: Group 'sda10f' is not a chain
[ERROR] [1620133674.989890394]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'sda10f'
[ERROR] [1620133674.989903274]: Kinematics solver could not be instantiated for joint group sda10f.
[ INFO] [1620133675.078926863]: Stereo is NOT SUPPORTED
[ INFO] [1620133675.079044093]: OpenGl version: 3,0 (GLSL 1,3).
[ INFO] [1620133675.082223199]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1620133675.085302546]: MoveGroup debug mode is OFF
Starting planning scene monitors...
[ INFO] [1620133675.085360924]: Starting planning scene monitor
[ INFO] [1620133675.087328427]: Listening to '/planning_scene'
[ INFO] [1620133675.087357798]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1620133675.089034103]: Listening to '/collision_object'
[ INFO] [1620133675.091377189]: Listening to '/planning_scene_world' for planning scene world geometry
[ERROR] [1620133675.092210069]: Failed to find 3D sensor plugin parameters for octomap generation
[ INFO] [1620133675.106962036]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1620133675.136309305]: Initializing OMPL interface using ROS parameters
[ INFO] [1620133675.167534248]: Using planning interface 'OMPL'
[ INFO] [1620133675.171473276]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1620133675.172205621]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1620133675.172794779]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1620133675.173385144]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1620133675.173809861]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1620133675.174239810]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1620133675.174314834]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1620133675.174378344]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1620133675.174396816]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1620133675.174412941]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1620133675.174445697]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1620133675.455734859]: Added FollowJointTrajectory controller for 
[ INFO] [1620133675.660351447]: Added FollowJointTrajectory controller for sda10f/sda10f_r1_controller
[ INFO] [1620133675.962271629]: Added FollowJointTrajectory controller for sda10f/sda10f_b1_controller
[ INFO] [1620133676.167056289]: Added FollowJointTrajectory controller for sda10f/sda10f_b2_controller
[ INFO] [1620133676.468327177]: Added FollowJointTrajectory controller for sda10f/sda10f_r2_controller
[ INFO] [1620133676.468627473]: Returned 5 controllers in list
[ INFO] [1620133676.509265466]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
[ERROR] [1620133676.530063985]: Exception while loading move_group capability 'move_group/MoveGroupExecuteService': MultiLibraryClassLoader: Could not create object of class type move_group::MoveGroupExecuteService as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary()
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1620133676.564497714]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1620133676.564598807]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1620133676.564628924]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1620133678.392524233]: Loading robot model 'motoman_sda10f'...
[ WARN] [1620133678.462648095]: Kinematics solver doesn't support #attempts anymore, but only a timeout.
Please remove the parameter '/rviz_remodel_TAU_4445_1580162335124445778/arm_left/kinematics_solver_attempts' from your configuration.
[ERROR] [1620133678.506206384]: Group 'torso' is not a chain
[ERROR] [1620133678.506250272]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'torso'
[ERROR] [1620133678.506340980]: Kinematics solver could not be instantiated for joint group torso.
[ERROR] [1620133678.506388257]: Group 'arms' is not a chain
[ERROR] [1620133678.506407846]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'arms'
[ERROR] [1620133678.506425580]: Kinematics solver could not be instantiated for joint group arms.
[ERROR] [1620133678.506470352]: Group 'sda10f' is not a chain
[ERROR] [1620133678.506488083]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'sda10f'
[ERROR] [1620133678.506509915]: Kinematics solver could not be instantiated for joint group sda10f.
[ INFO] [1620133678.622792723]: Starting planning scene monitor
[ INFO] [1620133678.624778120]: Listening to '/move_group/monitored_planning_scene'
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
[ INFO] [1620133679.686089540]: Constructing new MoveGroup connection for group 'arm_left' in namespace ''
[ INFO] [1620133680.822379648]: Ready to take commands for planning group arm_left.
[ INFO] [1620133680.822482462]: Looking around: no
[ INFO] [1620133680.822520338]: Replanning: no

As can be seen, there is an error saying that the ‘torso’, ‘arms’ and ‘sda10f’ groups are not a chain and that the kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for them. However, this is not an issue as reported in #70 and, additionally, I can move the ‘torso’ group without any problem.

Then, I call the robot_enable service in other terminal.

After that I am able to move the other three groups (‘left_arm’, ‘right_arm’ and ‘torso’) successfully. Then, I try to make a movement with the ‘arms’ group to a predefined joint position, it can plan the movement but, as can be seen in the screenshot below, the execution fails.

image

This is the code printed in the terminal when trying to execute the movement with the ‘arms’ group clicking in “Plan & Execute”.

[ INFO] [1620133726.747986102]: No active joints or end effectors found for group 'arms'. Make sure that kinematics.yaml is loaded in this node's namespace.
[ INFO] [1620133726.752448961]: Constructing new MoveGroup connection for group 'arms' in namespace ''
[ INFO] [1620133727.203286476]: Ready to take commands for planning group arms.
[ INFO] [1620133727.203343333]: Looking around: no
[ INFO] [1620133727.203375522]: Replanning: no
[ INFO] [1620133750.158852188]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1620133750.159792460]: Planning attempt 1 of at most 1
[ INFO] [1620133750.168230497]: Planner configuration 'arms' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1620133750.171689102]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.171862257]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.172047271]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.172381083]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.196769943]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1620133750.198952721]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1620133750.203780397]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1620133750.210835144]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1620133750.211418540]: ParallelPlan::solve(): Solution found by one or more threads in 0.041361 seconds
[ INFO] [1620133750.212523605]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.212665379]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.212751484]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.212835086]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.216593565]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1620133750.220724437]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1620133750.230353093]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1620133750.305990510]: RRTConnect: Created 11 states (5 start + 6 goal)
[ INFO] [1620133750.306199635]: ParallelPlan::solve(): Solution found by one or more threads in 0.094113 seconds
[ INFO] [1620133750.306513173]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.306602727]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1620133750.311435840]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1620133750.329402311]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1620133750.329593453]: ParallelPlan::solve(): Solution found by one or more threads in 0.023200 seconds
[ INFO] [1620133750.343977298]: SimpleSetup: Path simplification took 0.014204 seconds and changed from 3 to 2 states
[ INFO] [1620133750.345698323]: Disabling trajectory recording
[ERROR] [1620133750.429225068]: Aborting Trajectory.  Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)
[ERROR] [1620133751.561740416]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.203715 seconds). Stopping trajectory.
[ INFO] [1620133751.561923600]: Cancelling execution for 
[ INFO] [1620133751.562141028]: Completed trajectory execution with status TIMED_OUT ...
[ INFO] [1620133751.600360881]: ABORTED: Timeout reached
[ WARN] [1620133756.899362002]: Transitioning goal to LOST
[ WARN] [1620133756.899762672]: Controller  done, no result returned

The following was reported in http://www.smartroboticsys.eu/?p=675&lang=en in 2015 about the SDA10F ROS package: “What is still missing though, is MoveIT support for synchronized movement of arms/arms+torso respectively. It works fine in simulation, but due to some kind of MoveIT related error, the application fails when running on real robot”. In my case, I can also execute movements with the ‘arms’ group with the MoveIt demo (roslaunch motoman_sda10f_moveit_config demo.launch), but as was previously reported, it fails when running on real robot.

I would like to know which is the situation about this, and if a solution has been found for the execution of synchronized arms movements with the ‘arms’ or ‘sda10f’ groups.

Thank you.

Pablo.

@gavanderhoorn
Copy link
Member

Apologies for the delayed response.

I believe #259 works towards fixing this. It's not complete, but it should at least get you past the problem you report.

Could you try that PR and let us know whether it fixes the "goal lost" problem?

@gavanderhoorn gavanderhoorn added the multi-group Issues related to multi-group support label May 11, 2021
@pablomalvido
Copy link
Author

Thank you very much for the support. After making the modifications in the commits of #259 the simultaneous dual arm movement started working.

I have two additional questions, one is related with the synchronized dual arm movements in general (1) and the other just with the robot communication (2), but maybe you can help me also with them. I can open a new issue with these questions if you prefer:

  1. I want to do dual arm simultaneous cartesian movements. The only way I have found to this is by generating a cartesian plan for each arm individually and then merging them by code, adjusting the final duration to the duration of the plan that takes more time. I have tried the code with the real robot and it works fine, but there are some potential problems. As each individual plan is generated individually (using the compute_cartesian_path function), it doesn't consider potential self collisions between arms during the movements calculated in each plan. Another problem of this is that if I, for example, want to move both arms to the right 20cm but the distance between them is just 10cm, the left arm won't find a plan because at the moment of planning, the right arm is in the middle of the path. Is there any better solution for implementing this kind of movements?

  2. I would like to access to the torque value of the joints of the robot in ROS. Is this information published in any topic or accessible by calling any service?

Thank you very much.

Best regards,
Pablo.

@gavanderhoorn
Copy link
Member

I want to do dual arm simultaneous cartesian movements. The only way I have found to this is by generating a cartesian plan for each arm individually and then merging them by code, adjusting the final duration to the duration of the plan that takes more time. I have tried the code with the real robot and it works fine, but there are some potential problems. As each individual plan is generated individually (using the compute_cartesian_path function), it doesn't consider potential self collisions between arms during the movements calculated in each plan. Another problem of this is that if I, for example, want to move both arms to the right 20cm but the distance between them is just 10cm, the left arm won't find a plan because at the moment of planning, the right arm is in the middle of the path. Is there any better solution for implementing this kind of movements?

tbh, this is more a motion planning issue, instead of an issue with the driver.

You mention computeCartesianPath(..), which would make this a MoveIt question. I'd suggest discussing this with the MoveIt developers.

I would like to access to the torque value of the joints of the robot in ROS. Is this information published in any topic or accessible by calling any service?

while there is support for this in MotoROS, the messages used between MotoROS and motoman_driver do not allow us to send this data to the ROS side.

See #174 for an old discussion about this (as part of the work to add velocity feedback to the JointStates published by the driver).

In order to add this, we'd have to add additional SimpleMessage messages, which is a non-trivial amount of work.

@gavanderhoorn
Copy link
Member

@pablomalvido: to test a number of changes to motoman_driver, we're looking for users with SDA type robots willing to spend some time running some tests.

Would that be something you'd be interested in?

The tests are pretty low-level, with which I mean I expect they require very little time: the changes either work or they don't.

In case you'd be willing to help, it'll take me some time to make the test version available, so I'll get in touch with you somewhere next week most likely.

@gavanderhoorn
Copy link
Member

As it appears at least the initial problem has been addressed, and it looks like we're tracking the issues with multi-group motoman_driver setups in other issues already, I'm going to close the issue.

Feel free to keep commenting on it @pablomalvido.

@pablomalvido
Copy link
Author

Thank you again for the support @gavanderhoorn and yes, I would be happy to help to keep developing the repository. So just contact me whenever you need to run the tests.

Have a nice weekend,
Pablo.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
multi-group Issues related to multi-group support
Development

No branches or pull requests

2 participants