Error when switching controllers

[devraise]

Affected ROS Driver version(s)

2.1.2

Used ROS distribution.

Noetic

Which combination of platform is the ROS driver running on.

Linux without realtime patch

How is the UR ROS Driver installed.

Build both the ROS driver and UR Client Library from source

Which robot platform is the driver connected to.

UR E-series robot

Robot SW / URSim version(s)

5.14.2

How is the ROS driver used.

Headless without using the teach pendant

Issue details

Summary

I'm trying to use a cartesian motion controller found at https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers with the UR20. I have been able to successfully use this with the UR10E series. However, when I try to use this with the UR20, I receive the error below.

Issue details

I try to switch the controller using this command

rosservice call /right_arm/controller_manager/switch_controller "start_controllers: ['my_cartesian_motion_controller']
stop_controllers: ['pos_joint_traj_controller']
strictness: 0
start_asap: true
timeout: 0.0" 

I receive the following error:

ur_robot_driver_node: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:425: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index) [with Derived = Eigen::Matrix<double, -1, 1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]: Assertion `index >= 0 && index < size()' failed.

Steps to Reproduce

Clone the cartesian controller repo.

Make a config file with the following:

my_cartesian_motion_controller:
  type: "position_controllers/CartesianMotionController"
  end_effector_link: "right_fastener_gripper_tip"
  robot_base_link: "base_link"
  target_frame_topic: "target_frame"
  solver:
    error_scale: 1.0
    iterations: 1
  joints:
    - right_shoulder_pan_joint
    - right_shoulder_lift_joint
    - right_elbow_joint
    - right_wrist_1_joint
    - right_wrist_2_joint
    - right_wrist_3_joint
  pd_gains:
    trans_x: {p: 1.0}
    trans_y: {p: 1.0}
    trans_z: {p: 1.0}
    rot_x: {p: 0.1}
    rot_y: {p: 0.1}
    rot_z: {p: 0.1}

Spawn the controller using the following launch file:

<launch>
    <rosparam file="$(find metalman_planning)/config/cartesian_controllers.yaml" command="load"/>

    <!-- Spawn controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner"
        args="--stopped
        my_cartesian_motion_controller
        "
    />
</launch>

Switch controller with

rosservice call /right_arm/controller_manager/switch_controller "start_controllers: ['my_cartesian_motion_controller']
stop_controllers: ['pos_joint_traj_controller']
strictness: 0
start_asap: true
timeout: 0.0" 

Expected Behavior

Controller should have switched from pos_joint_traj_controller to my_cartesian_motion_controller

Actual Behavior

Received error:

ur_robot_driver_node: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:425: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index) [with Derived = Eigen::Matrix<double, -1, 1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]: Assertion `index >= 0 && index < size()' failed.

Relevant log output

No response

Accept Public visibility

• [X] I agree to make this context public

[RobertWilbrandt]

I just tried using your controller config on both a ur16e and a ur20 and (using tool0 as end_effector_link instead of right_fastener_gripper_tip) was able to switch controllers and control the robot using the /my_cartesian_motion_controller/target_frame topic. I therefore suspect that the problem here stems from either your custom URDF setup (do you for example have additional joints between tool0 and right_fastener_gripper_tip that are not listed?) or some problem within CartesianMotionController. Do you have a reason to think that this problem stems from the ur20 description? Otherwise i would suggest filing an issue in the cartesian_controllers repo instead.

[fmauch]

I'll mark this for getting closed if no further information is provided. If there's another issue related to this, please post it here.