0
$\begingroup$

Rosanswers logo

I am trying to simulate multiple universal robots arm on a mobile robot.

To achieve this first I am trying to get the single arm under one namespace(center). I am to run the simulation and the robot in Gaebo(from URDF) is spawned correctly and the controller manager is also launched in its respective namespace. However arm_controller is not launched as the controller manager is not able to find arm_controller however it already is going in its namespace center/arm_controller.

Here are the changes I have made:

UR5e.launch: Modified UR5e.launch

ur5e_robot.urdf.xacro: Modified ur5e.urdf.xacro

Common.gazebo.xacro: Modified common.gazebo.xacro

Here is the logging info:

... logging to /home/student/.ros/log/d738a7f2-aaff-11ea-af39-913d8e648243/roslaunch-Student-W35xSS-370SS-32725.log

Checking log directory for disk usage. This may take awhile.

Press Ctrl-C to interrupt

Done checking log file disk usage. Usage is <1GB.



inconsistent namespace redefinitions for xmlns:xacro:

 old: http://ros.org/wiki/xacro

 new: http://wiki.ros.org/xacro (/home/student/tud_ws/src/fmauch_universal_robot/ur_e_description
/urdf/ur.transmission.xacro)

started roslaunch server http://Student-W35xSS-370SS:36449/


SUMMARY

========



PARAMETERS

 * /center/arm_controller/action_monitor_rate: 10

 * /center/arm_controller/constraints/elbow_joint/goal: 0.1

 * /center/arm_controller/constraints/elbow_joint/trajectory: 0.1

 * /center/arm_controller/constraints/goal_time: 0.6

 * /center/arm_controller/constraints/shoulder_lift_joint/goal: 0.1

 * /center/arm_controller/constraints/shoulder_lift_joint/trajectory: 0.1

 * /center/arm_controller/constraints/shoulder_pan_joint/goal: 0.1

 * /center/arm_controller/constraints/shoulder_pan_joint/trajectory: 0.1

 * /center/arm_controller/constraints/stopped_velocity_tolerance: 0.05

 * /center/arm_controller/constraints/wrist_1_joint/goal: 0.1

 * /center/arm_controller/constraints/wrist_1_joint/trajectory: 0.1

 * /center/arm_controller/constraints/wrist_2_joint/goal: 0.1

 * /center/arm_controller/constraints/wrist_2_joint/trajectory: 0.1

 * /center/arm_controller/constraints/wrist_3_joint/goal: 0.1

 * /center/arm_controller/constraints/wrist_3_joint/trajectory: 0.1

 * /center/arm_controller/joints: ['shoulder_pan_jo...

 * /center/arm_controller/state_publish_rate: 25

 * /center/arm_controller/stop_trajectory_duration: 0.5

 * /center/arm_controller/type: position_controll...

 * /center/joint_group_position_controller/joints: ['shoulder_pan_jo...

 * /center/joint_group_position_controller/type: position_controll...

 * /center/joint_state_controller/publish_rate: 50

 * /center/joint_state_controller/type: joint_state_contr...

 * /center/robot_state_publisher/publish_frequency: 50.0

 * /center/robot_state_publisher/tf_prefix: 

 * /robot_description: <?xml version="1....

 * /rosdistro: kinetic

 * /rosversion: 1.12.14

 * /use_sim_time: True


NODES

  /center/

    arm_controller_spawner (controller_manager/controller_manager)

    fake_joint_calibration (rostopic/rostopic)

    joint_state_controller_spawner (controller_manager/controller_manager)

    robot_state_publisher (robot_state_publisher/robot_state_publisher)

    ros_control_controller_manager (controller_manager/controller_manager)

  /
    gazebo (gazebo_ros/gzserver)

    gazebo_gui (gazebo_ros/gzclient)

    spawn_gazebo_model (gazebo_ros/spawn_model)

ROS_MASTER_URI=http://localhost:11311

process[gazebo-1]: started with pid [32745]

process[gazebo_gui-2]: started with pid [32750]

process[center/robot_state_publisher-3]: started with pid [32755]

process[center/fake_joint_calibration-4]: started with pid [32756]

process[center/joint_state_controller_spawner-5]: started with pid [32758]

process[center/arm_controller_spawner-6]: started with pid [309]

process[center/ros_control_controller_manager-7]: started with pid [318]


process[spawn_gazebo_model-8]: started with pid [332]

[ INFO] [1591783185.571660594]: Finished loading Gazebo ROS API Plugin.

[ INFO] [1591783185.574029289]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, 
waiting...

[ INFO] [1591783185.642075820]: Finished loading Gazebo ROS API Plugin.

[ INFO] [1591783185.642815770]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, 
waiting...

SpawnModel script started

[INFO] [1591783185.938174, 0.000000]: Loading model XML from ros parameter

[INFO] [1591783185.941604, 0.000000]: Waiting for service /gazebo/spawn_urdf_model

[ INFO] [1591783186.028815169, 0.023000000]: waitForService: Service [/gazebo/set_physics_properties] is now 
available.

[ INFO] [1591783186.084236189, 0.073000000]: Physics dynamic reconfigure ready.

[INFO] [1591783186.245369, 0.232000]: Calling service /gazebo/spawn_urdf_model

[ INFO] [1591783186.262621625, 0.250000000]: waitForService: Service [/gazebo/set_physics_properties] is now 
available.

[INFO] [1591783186.432241, 0.346000]: Spawn status: SpawnModel: Successfully spawned entity

[ INFO] [1591783186.452998694, 0.346000000]: Physics dynamic reconfigure ready.

[ INFO] [1591783186.472694215, 0.346000000]: Loading gazebo_ros_control plugin

[ERROR] [1591783186.472804202, 0.346000000]: GazeboRosControlPlugin missing <legacyModeNS> while using 
DefaultRobotHWSim, defaults to true.

This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, where the 
robotNamespace is disregarded and absolute paths are used instead.

If you do not want to fix this issue in an old package just set <legacyModeNS> to true.


[ INFO] [1591783186.473480857, 0.346000000]: Starting gazebo_ros_control plugin in namespace: center/

[ INFO] [1591783186.474362502, 0.346000000]: gazebo_ros_control plugin is waiting for model URDF in parameter 
[/robot_description] on the ROS param server.

[ERROR] [1591783186.595263836, 0.346000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control
/pid_gains/center/shoulder_pan_joint

[ERROR] [1591783186.597371286, 0.346000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control
/pid_gains/center/shoulder_lift_joint

[ERROR] [1591783186.600219593, 0.346000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control
/pid_gains/center/elbow_joint

[ERROR] [1591783186.602013462, 0.346000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control
/pid_gains/center/wrist_1_joint

[ERROR] [1591783186.603294394, 0.346000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control
/pid_gains/center/wrist_2_joint

[ERROR] [1591783186.604687222, 0.346000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control
/pid_gains/center/wrist_3_joint

[ INFO] [1591783186.611954993, 0.346000000]: Loaded gazebo_ros_control.

[ WARN] [1591783186.612174365, 0.347000000]: The default_robot_hw_sim plugin is using the Joint::SetPosition 
method without preserving the link velocity.

[ WARN] [1591783186.612204661, 0.347000000]: As a result, gravity will not be simulated correctly for your model.

[ WARN] [1591783186.612228275, 0.347000000]: Please set gazebo_pid parameters, switch to the 
VelocityJointInterface or EffortJointInterface, or upgrade to Gazebo 9.

[ WARN] [1591783186.612255812, 0.347000000]: For details, see https://github.com/ros-simulation/gazebo_ros_pkgs
/issues/612

Loaded joint_state_controller

Started ['joint_state_controller'] successfully

[spawn_gazebo_model-8] process has finished cleanly

log file: /home/student/.ros/log/d738a7f2-aaff-11ea-af39-913d8e648243/spawn_gazebo_model-8*.log

[ERROR] [1591783186.742369638, 0.457000000]: Exception thrown: Could not find resource 'shoulder_pan_joint' in 
'hardware_interface::PositionJointInterface'.

[ERROR] [1591783186.742459874, 0.457000000]: Failed to initialize the controller

[ERROR] [1591783186.742524898, 0.457000000]: Initializing controller 'joint_group_position_controller' failed

Error when loading joint_group_position_controller

[center/joint_state_controller_spawner-5] process has finished cleanly
log file: /home/student/.ros/log/d738a7f2-aaff-11ea-af39-913d8e648243/center-joint_state_controller_spawner-5*.log


[ERROR] [1591783186.893482385, 0.604000000]: Could not load controller 'center/arm_controller' because the type was not specified. Did you load the controller configuration on the parameter server (namespace: '/center/center
/arm_controller')?

Error when loading center/arm_controller

[ERROR] [1591783186.910529995, 0.609000000]: Could not start controller with name center/arm_controller because 
no controller with this name exists

Error when starting  ['center/arm_controller']

[center/ros_control_controller_manager-7] process has finished cleanly

log file: /home/student/.ros/log/d738a7f2-aaff-11ea-af39-913d8e648243/center-ros_control_controller_manager-
7*.log

[center/arm_controller_spawner-6] process has finished cleanly

log file: /home/student/.ros/log/d738a7f2-aaff-11ea-af39-913d8e648243/center-arm_controller_spawner-6*.log

libGL error: failed to create drawable

Originally posted by Tahir M. on ROS Answers with karma: 213 on 2020-06-10

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The above mentioned steps are OK below are the steps needed to get everything working.

I was able to solve the problem by changing the parameters of the arm_controller in the yaml file to the respective namespace.

In original file they are hardcoded:

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
      elbow_joint: {trajectory: 0.1, goal: 0.1}
      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10
joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint

So I appended namespace with them:

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - center/shoulder_pan_joint
     - center/shoulder_lift_joint
     - center/elbow_joint
     - center/wrist_1_joint
     - center/wrist_2_joint
     - center/wrist_3_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
      elbow_joint: {trajectory: 0.1, goal: 0.1}
      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10
joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
     - center/shoulder_pan_joint
     - center/shoulder_lift_joint
     - center/elbow_joint
     - center/wrist_1_joint
     - center/wrist_2_joint
     - center/wrist_3_joint

Also changed the tf_prefix to center


Originally posted by Tahir M. with karma: 213 on 2020-06-10

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.