Document usage on custom robot

[stefanscherzinger]

Goal

Users new to both ROS and ROS-control are sometimes unsure how to adjust the examples to their real robot at hand. Although we can't provide a ROS-tutorial here, it would be nice to have some tips, concerning

• How to adjust joint names in the controllers and compose their own .config file
• Where to spawn them so that their robot's controller_manager can find them
• What to load, and what to load and start
• Useful steps that I normally check to see if everything is working as expected (joint_states, TF tree, rqt-joint-trajectory-controller, motion_control_handle, cartesian_motion_controller, ...)

---

Edit: I created a getting started package here for a Universal Robots UR10e.

[Mohatashem]

Hello @stefanscherzinger,

I was wondering whether I should pose under this issue, but since the questions I ask pertain to the content of the ReadMes in each case, so here it is. I intend to use cartesian controllers in gazebo and on two UR10e real robots. I have been trying to understand a few things about the repo so as to document their use and implement on our robots:

1. What is the difference between the topics /target_wrench and /my_cartesian_force_controller/ft_sensor_wrench?
   For each tutorial, it is instructed to play with the solver/error_scale. But I can’t seem to find the error_scale anywhere. See pic:
   

2. For force controller, in the step:
   "Publish a geometry_msgs/WrenchStamped to /my_cartesian_force_controller/target_wrench with force x = 2 and watch the robot move"

But I can't seem to find this topic. The only wrench related topics I find after doing a rostopic list | grep wrench are:

/my_cartesian_compliance_controller/ft_sensor_wrench
/my_cartesian_force_controller/ft_sensor_wrench
/target_wrench

So, I tested by publishing to both the /target_wrench and to /my_cartesian_force_controller/ft_sensor_wrench. The robot moves in both cases, but what is the topic I should be publishing to, to simulate the actual case?

3. For compliance controller, just like in no. 2, we can observe the behaviour of this controller. We can clearly see what happens on changing the stiffness regardless of whether we publish to /target_wrench or to /my_cartesian_compliance_controller/ft_sensor_wrench, but the question stays the same, what is the topic I should be publishing to to simulate the actual case.

4. Lastly, when I launch the examples.launch folder, everything seems alright in rviz but I see these messages in the terminal:

[ INFO] [1644400474.938960185] [/sim_hardware_interface] [ros.cartesian_force_controller]: Failed to load /my_cartesian_compliance_controller/gravity from parameter server
[ INFO] [1644400474.939347612] [/sim_hardware_interface] [ros.cartesian_force_controller]: Failed to load /my_cartesian_compliance_controller/tool from parameter server

Everything seems to work fine despite 4. But should we be worried about it?

[stefanscherzinger]

Hi @Mohatashem

That's ok to post it here.

1. What is the difference between the topics /target_wrench and /my_cartesian_force_controller/ft_sensor_wrench?

The /target_wrench is the desired wrench vector that your robot should apply to its environment via the specified end-effector. In terms of control theory, it's the reference signal. Closed-loop control needs to measure the actual contact forces to compute error correction. This is the ft_sensor_wrench, in control theory the controlled variable.

But I can’t seem to find the error_scale anywhere.

It's a little bit hidden. It's part of the solver namespace for each controller:

2. ... But I can't seem to find this topic.

You are right. Thanks for pointing this out. I guess I remapped both the cartesian_force_controller and the cartesian_compliance_controller to work with the /target_wrench in the example, so that's easier to play with them.

but what is the topic I should be publishing to, to simulate the actual case?

The /target_wrench is the right topic to publish to. I'll update the README.md. Note that this is sufficient for steering the robot around a little. If you want to simulate contacts (or once you work on the real UR10e), you need a sensor that publishes force-torque measurements on the respective ft_sensor_wrench topic.

3. For compliance controller ...

That's the same case. See 2..

4. ... But should we be worried about it?

No, you needn't worry. Ignore that for now. I need to rework that feature a little (#40 ).

[Mohatashem]

@stefanscherzinger
Thank you so very much for your explanation. There are a couple other points that I'd like to clarify if it is alright. This one might be unrelated but since it's mentioned in one of the papers that it is manipulator agnostic, does it mean we could use it with a robot having axes > 6 (or if I want to add an additional rail to our robot, it's something I intend in the near future)?

The second point, I've managed to implement it with my setup in rviz, just following the tutorials and your explanation above. However, when trying to implement the same in gazebo, the cartesian_motion_controller and the cartesian_force_controller seem to work fine. Even the cartesian_compliance_controller seems working fine when applying a target_wrench and testing different values for stiffness. However, when trying to move interactive markers the robot does not move along. I am not sure what exactly to check for in this case. I am using the same set of controllers that I used for the sim_hardware_interface in rviz. Here is an extract from my yaml file:

$(arg ceiling_prefix)my_cartesian_motion_controller:
    type: "position_controllers/CartesianMotionController"
    end_effector_link: "$(arg ceiling_prefix)tool0"
    robot_base_link: "$(arg ceiling_prefix)base_link"
    target_frame_topic: "target_frame"
    joints: 
    - $(arg ceiling_prefix)shoulder_pan_joint
    - $(arg ceiling_prefix)shoulder_lift_joint
    - $(arg ceiling_prefix)elbow_joint
    - $(arg ceiling_prefix)wrist_1_joint
    - $(arg ceiling_prefix)wrist_2_joint
    - $(arg ceiling_prefix)wrist_3_joint

    pd_gains:
        trans_x: {p: 10.0}
        trans_y: {p: 10.0}
        trans_z: {p: 10.0}
        rot_x: {p: 1.0}
        rot_y: {p: 1.0}
        rot_z: {p: 1.0}

$(arg ceiling_prefix)my_cartesian_force_controller:
    type: "position_controllers/CartesianForceController"
    end_effector_link: "$(arg ceiling_prefix)tool0"
    robot_base_link: "$(arg ceiling_prefix)base_link"
    ft_sensor_ref_link: "$(arg ceiling_prefix)force_sensor_link"
    joints: 
    - $(arg ceiling_prefix)shoulder_pan_joint
    - $(arg ceiling_prefix)shoulder_lift_joint
    - $(arg ceiling_prefix)elbow_joint
    - $(arg ceiling_prefix)wrist_1_joint
    - $(arg ceiling_prefix)wrist_2_joint
    - $(arg ceiling_prefix)wrist_3_joint

    pd_gains:
        trans_x: {p: 0.05}
        trans_y: {p: 0.05}
        trans_z: {p: 0.05}
        rot_x: {p: 0.01}
        rot_y: {p: 0.01}
        rot_z: {p: 0.01}

$(arg ceiling_prefix)my_cartesian_compliance_controller:
    type: "position_controllers/CartesianComplianceController"
    end_effector_link: "$(arg ceiling_prefix)tool0"
    robot_base_link: "$(arg ceiling_prefix)base_link"
    ft_sensor_ref_link: "$(arg ceiling_prefix)force_sensor_link"
    compliance_ref_link: "$(arg ceiling_prefix)tool0"
    target_frame_topic: "target_frame"
    joints: 
    - $(arg ceiling_prefix)shoulder_pan_joint
    - $(arg ceiling_prefix)shoulder_lift_joint
    - $(arg ceiling_prefix)elbow_joint
    - $(arg ceiling_prefix)wrist_1_joint
    - $(arg ceiling_prefix)wrist_2_joint
    - $(arg ceiling_prefix)wrist_3_joint

    stiffness:
        trans_x: 500
        trans_y: 500
        trans_z: 500
        rot_x: 100
        rot_y: 100
        rot_z: 100

    pd_gains:
        trans_x: {p: 0.05}
        trans_y: {p: 0.05}
        trans_z: {p: 0.05}
        rot_x: {p: 0.01}
        rot_y: {p: 0.01}
        rot_z: {p: 0.01}

$(arg ceiling_prefix)my_motion_control_handle:
   type: "cartesian_controllers/MotionControlHandle"
   end_effector_link: "$(arg ceiling_prefix)tool0"
   robot_base_link: "$(arg ceiling_prefix)base_link"
   target_frame_topic: "/$(arg ceiling_prefix)my_cartesian_motion_controller/target_frame"
   joints: 
    - $(arg ceiling_prefix)shoulder_pan_joint
    - $(arg ceiling_prefix)shoulder_lift_joint
    - $(arg ceiling_prefix)elbow_joint
    - $(arg ceiling_prefix)wrist_1_joint
    - $(arg ceiling_prefix)wrist_2_joint
    - $(arg ceiling_prefix)wrist_3_joint

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

and my launch file is here:

<launch>

        <!--Gazebo related Args-->
        <arg name="gui" default="true"/>
        <arg name="paused" default="true"/>
        <arg name="use_sim_time" default="true"/>

        <arg name="model" default="$(find zerog_interaction_control)/urdf/zerog_interaction.xacro"/>
        <!-- Configuration -->
        <arg name="rviz" default="true"/>
        <arg name="debug" default="false"/>
        <arg if="$(arg debug)" name="launch-prefix" value="screen -d -m gdb -command=$(env HOME)/.ros/my_debug_log --ex run --args"/>
        <arg unless="$(arg debug)" name="launch-prefix" value=""/>
        <arg name="headless" default="false"/>
        <arg name="ceiling_prefix" default="ceiling_arm_"/>
        <arg name="wall_prefix" default="wall_arm_"/>
        <arg name="ceiling_track_prefix" default="ceiling_track_"/>
        <arg name="wall_track_prefix" default="wall_track_"/>        

        <!-- We start in an empty_world.launch, changing only the name of the world to be launched if needed-->
        <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <!-- <arg name="world_name" value="$(arg world_name)"/> -->
                <arg name="debug" value="$(arg debug)" />
                <arg name="gui" value="$(arg gui)" />
                <arg name="paused" value="$(arg paused)"/>
                <arg name="use_sim_time" value="$(arg use_sim_time)"/>
                <arg name="headless" value="$(arg headless)"/>
        </include>



        <!-- Load robot_description to parameter server -->

          <param name="/robot_description" command="$(find xacro)/xacro '$(arg model)'
                wall_prefix:=$(arg wall_prefix)
                ceiling_prefix:=$(arg ceiling_prefix)
                wall_track_prefix:=$(arg wall_track_prefix)
                ceiling_track_prefix:=$(arg ceiling_track_prefix)" />


        <!-- Load hardware configuration -->
        <!-- <rosparam file="$(find zerog_interaction_control)/config/hardware_config.yaml" command="load" subst_value="True">$(arg ceiling_prefix),$(arg wall_prefix),$(arg ceiling_track_prefix),$(arg wall_track_prefix)</rosparam> -->

        <!-- Load hardware interface -->
        <!-- <node name="sim_hardware_interface" pkg="ros_control_boilerplate" type="sim_hw_main" output="screen" launch-prefix="$(arg launch-prefix)" /> -->

        <!-- Robot state publisher -->
        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
                <param name="publish_frequency" type="double" value="50.0" />
                <param name="tf_prefix" type="string" value="" />
        </node>

        <!-- Joint state publisher -->
        <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" respawn="true" output="screen"/>

        <!-- Load controller configuration -->
        <rosparam file="$(find zerog_interaction_control)/config/all_cartesian_controllers.yaml" command="load" subst_value="True">$(arg ceiling_prefix),$(arg wall_prefix),$(arg ceiling_track_prefix),$(arg wall_track_prefix)</rosparam>

        <!-- Spawn controllers -->
        <node name="controller_spawner" pkg="controller_manager" type="spawner"
                args="--stopped ceiling_arm_my_cartesian_motion_controller ceiling_arm_my_cartesian_force_controller ceiling_arm_my_cartesian_compliance_controller ceiling_arm_my_motion_control_handle
               "/>

        <!-- Spawn joint controller for testing my_joint_to_cartesian_controller  -->
        <!-- Note: This controller should be loaded and started -->
        <!-- <group ns="my_joint_to_cartesian_controller" >
                <node name="controller_spawner" pkg="controller_manager" type="spawner" args="joint_trajectory_controller" />
        </group> -->

        <!-- MRM: 08/02/2022: Starting to load zerog system with desired initial configuration to avoid unwanted starting poses. -->
        <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -param robot_description -model zerog_interaction 
        -J ceiling_track_slider_joint 0.0
        -J ceiling_arm_shoulder_pan_joint 0.0 
        -J ceiling_arm_shoulder_lift_joint -1.57
        -J ceiling_arm_elbow_joint -1.95
        -J ceiling_arm_wrist_1_joint -1.57
        -J ceiling_arm_wrist_2_joint 1.57 
        -J ceiling_arm_wrist_3_joint 0.0
        -J wall_track_slider_joint 0.0 
        -J wall_arm_shoulder_pan_joint 0.0 
        -J wall_arm_shoulder_lift_joint 0.0 
        -J wall_arm_elbow_joint 0.0 
        -J wall_arm_wrist_1_joint -1.57 
        -J wall_arm_wrist_2_joint 1.57 
        -J wall_arm_wrist_3_joint 0.0
        -unpause"/> 



        <!-- Visualization -->
        <group if="$(arg rviz)">
                <arg name="my_rviz" value="-d $(find zerog_interaction_control)/etc/interaction.rviz" />
                <node name="rviz" pkg="rviz" type="rviz" respawn="false" args="$(arg my_rviz)" output="screen" />
        </group>
</launch>

And since I am not doing any remapping, Inside rviz, I am using the ceiling_arm_my_cartesian_controller/target_frame for the Pose and /ceiling_arm_my_motion_control_handle/update for the InteractiveMarkers. Would you have any suggestions?

[stefanscherzinger]

@Mohatashem

does it mean we could use it with a robot having axes > 6

Yes, that works. But use the devel branch for this. We need an additional joint damping to suppress null space motion for redundant manipulators. Alternatively, you could switch the controller's IK-Solver and see if that works better in your case.

or if I want to add an additional rail to our robot,

That's a bit more tricky. I remember that a colleague of mine tried this and it did not work out-of-the-box. You could try that in simulation first. If it doesn't work, I would like to help debugging and fixing this. Thinking about this, I guess you need some sort of combined hardware interface, since the controllers must claim the handles for the joints and the rail.

However, when trying to move interactive markers the robot does not move along. I am not sure what exactly to check for in this case.

Hm. You seem to be using yaml arguments a lot. I don't have much experience with this, but I would debug the classic way:

1. Once the ceiling_arm_my_motion_control_handle is active, it publishes a target to some topic.
2. Find that topic and check if it is connected to your controller with rostopic info <topic>.
3. rostopic echo to that topic and check if you get new values from RViz.
4. If that's not the case, check the marker's update topic with rostopic info <topic>.
5. Somewhere in your setup, information does not propagate correctly.

This is the high-level information flow that needs to be connected:
Rviz -> Interactive marker -> motion_control_handle -> cartesian_motion_controller.

[Mohatashem]

Thanks again for your suggestions. I do apologize if I am pushing this discussion too long.

I guess you need some sort of combined hardware interface, since the controllers must claim the handles for the joints and the rail.

Yes, I am currently trying to wrap my head around this. I have already written a hardware_interface for the rails and they work quite alright. I am studying on how I can integrate it with the UR. Will probably keep you posted as we want to incorporate the cartesian_controllers as well.

Besides that, I am wondering whether there is anything else we need to do when using gazebo urdf spawner instead of the sim_hardware_interface node? You are right about the information not propagating correctly.

This is the rqt_graph when using the sim_hardware_interface node in the launch file:

Case 1: my setup in rviz

This is the successful case.

• The motion handle was active with the compliance controller.
• Doing a rostopic echo on both /ceiling_arm_my_motion_control_handle/update and /ceiling_arm_my_motion_control_handle/feedback and there output was alright, i.e. would change when moving the interactiveMarkers in Rviz.

The above case is similar to running the examples.launch in terms of working:

Case 2: examples.launch

• Note that all the namespace/target_frame and namespace/target_wrench topics have been remapped to target_frame and target_wrench successfully.

Case 3: With Gazebo

• I loaded the same prefixed controllers used in Case 1 (mentioned in the previous post) (There are few things I observed when using with gazebo (launch file in comment)
  
  Again, on activation of the /ceiling_arm_my_motion_control_handle the /ceiling_arm_my_motion_control_handle/update and /ceiling_arm_my_motion_control_handle/feedback worked just like in Case 1. In this case the /ceiling_arm_my_cartesian_motion_controller/target_frame does publish a geometry_msgs/PoseStampedbut/ceiling_arm_my_cartesian_compliance_controller/target_frameand the/target_frame` do not.
  

I even tried remapping:

<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -param robot_description -model zerog_interaction 
        -J ceiling_track_slider_joint 0.0
        -J ceiling_arm_shoulder_pan_joint 0.0 
        -J ceiling_arm_shoulder_lift_joint -1.57
        -J ceiling_arm_elbow_joint -1.95
        -J ceiling_arm_wrist_1_joint -1.57
        -J ceiling_arm_wrist_2_joint 1.57 
        -J ceiling_arm_wrist_3_joint 0.0
        -J wall_track_slider_joint 0.0 
        -J wall_arm_shoulder_pan_joint 0.0 
        -J wall_arm_shoulder_lift_joint 0.0 
        -J wall_arm_elbow_joint 0.0 
        -J wall_arm_wrist_1_joint -1.57 
        -J wall_arm_wrist_2_joint 1.57 
        -J wall_arm_wrist_3_joint 0.0
        -unpause">  
        
        <remap from="$(arg ceiling_prefix)my_motion_control_handle/target_frame" to="target_frame"/>
        <remap from="$(arg ceiling_prefix)my_cartesian_motion_controller/target_frame" to="target_frame"/>
        <remap from="$(arg ceiling_prefix)my_cartesian_compliance_controller/target_frame" to="target_frame"/>
        <!-- <!- Control wrenches via one topic -> -->
        
        <remap from="$(arg ceiling_prefix)my_cartesian_force_controller/target_wrench" to="target_wrench" />
        <remap from="$(arg ceiling_prefix)my_cartesian_compliance_controller/target_wrench" to="target_wrench" /> 
     
        </node> 

But apparently, it does not work, as the rqt_graph stays the same, and I get the same behaviour with rostopic echo as without remapping

[stefanscherzinger]

@Mohatashem

Will probably keep you posted as we want to incorporate the cartesian_controllers as well.

Sounds interesting. I'm curious to see how that turns out in the end.

Concerning you questions:

Case 3: With Gazebo ...
In this case the /ceiling_arm_my_cartesian_motion_controller/target_frame does publish a geometry_msgs/PoseStampedbut/ceiling_arm_my_cartesian_compliance_controller/target_frameand the/target_frame` do not.

Just to be clear, by does publish and do not, you mean that when rostopic echo on the first, you receive something, and on the second and third you don't, right? At least that's what you screenshot from the terminal says.

I even tried remapping:

Yes, that's always a bit tricky. But it does work correctly. Just try different things until you get there. You could adapt your remaps to the global /target_frame. If that works, you can investigate from there.

---

Edit: I think it's worth taking the time to understand remaping..

[Mohatashem]

@stefanscherzinger
Yes, Thank you so much for the suggestions. It is much appreciated.

You could adapt your remaps to the global /target_frame. If that works, you can investigate from there.

Testing around a bit, I got it to work. Apart from that, my $(arg ceiling_prefix)my_motion_control_handle's target_frame_topic was incorrect.

To summarize (just in case someone else finds it useful for setting it up with gazebo):

1. Setting the target_frame_topic under concerned controller in the yaml file to /target_frame.
2. Apart from that, I needed to remap under the spawner node as follows:

<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -param robot_description -model zerog_interaction 
        -J ceiling_track_slider_joint 0.0
        -J ceiling_arm_shoulder_pan_joint 0.0 
        -J ceiling_arm_shoulder_lift_joint -1.57
        -J ceiling_arm_elbow_joint -1.95
        -J ceiling_arm_wrist_1_joint -1.57
        -J ceiling_arm_wrist_2_joint 1.57 
        -J ceiling_arm_wrist_3_joint 0.0
        -J wall_track_slider_joint 0.0 
        -J wall_arm_shoulder_pan_joint 0.0 
        -J wall_arm_shoulder_lift_joint 0.0 
        -J wall_arm_elbow_joint 0.0 
        -J wall_arm_wrist_1_joint -1.57 
        -J wall_arm_wrist_2_joint 1.57 
        -J wall_arm_wrist_3_joint 0.0
        -unpause">  
        
        <remap from="$(arg ceiling_prefix)my_motion_control_handle/target_frame" to="/target_frame"/>
        <remap from="$(arg ceiling_prefix)my_cartesian_motion_controller/target_frame" to="/target_frame"/>
        <remap from="$(arg ceiling_prefix)my_cartesian_compliance_controller/target_frame" to="/target_frame"/>

</node> 

3. If remapping is done correctly, /target_frame should have the publishers and subscribers as follows:

Publishers: 
 * /gazebo 
Subscribers: 
 * /rviz 
 * /gazebo

[Mohatashem]

Hello @stefanscherzinger,

I succeeded in implementing the controllers on the real robot (UR10e mounted on a ceiling). I've tested the motion, compliance and force controllers. But I have a few follow-up questions/doubts. I hope you could kindly clarify them for me:

1. Using the cartesian_force controller, I have observed the following:

• On starting pressing the play button from the teach pendant to start ros control, the ft_sensor values are all zeroed.
• Then the robot is guided by hand and it moves.
• For most trials, as long as the orientation of the tool stays same, the robot maintains it’s position when the applied force is removed.
• But in case the robot’s orientation changes under the influence of hand guiding, the robot end-effector starts to sink to the floor, once the applied force is removed.

This is a phenomenon I observed, with and without the end-effector attached. And before starting the controller, I make sure from the teach pendant side, to set the appropriate TCP and tool COM and payload.

Q: How can this be avoided? Is there a calibration procedure, I need to run on ROS side? Is there anything else I am missing?

2. Using compliance control, I was testing the robot for possible implementation of "free-floating" behaviour when pushing the end-effector with hand. My idea was to have a highest compliance with high responsiveness.

The stiffness values chosen were the lowest possible specified in the dynamic_reconfigure:

stiffness:  # w.r.t. compliance_ref_link
        trans_x: 100
        trans_y: 100
        trans_z: 100
        rot_x: 5
        rot_y: 5
        rot_z: 5

Is it recommended stay within the range specified in the repo? Would it be safe to go lower than these values?

Additionally, I noticed that for the real robot, to run cartesian_compliance_controller, we need to reduce the number of iterations to '1'. This is something mentioned here as well.

Q: Does it mean that for cartesian_compliance_controller, we always need to set iterations = 1?

So far for me, setting iterations > 1, the robot begins to oscillate right away. And since, it is recommended to build in release mode and we are using catkin build instead of catkin_make, I did the following:

catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build

I am assuming this would have the same result as

catkin_make -DCMAKE_BUILD_TYPE=Release

Q: Is the above mentioned method correct/recommended?

For these tests I had set the error_scale to 1, and the gains were as follows:

pd_gains:
    trans_x: {p: 0.135}
    trans_y: {p: 0.135}
    trans_z: {p: 0.135}
    rot_x: {p: 1.3}
    rot_y: {p: 1.3}
    rot_z: {p: 1.3}

the reason why, in pd_gains, my rot_ values are higher than trans_ are because that's how I was able to achieve more "responsive" rotational compliance at the robot flange. Increasing the pd_gains, any further and the robot begins to oscillate slightly as soon as touched, but otherwise remains fluid and without oscillations, as soon as I let go. So I guess, for now that would be the "optimal" limit for me.

Apart from everything above:
3. Q: What is the utility of the following controllers in the yaml files?

my_joint_to_cartesian_controller
my_joint_to_cartesian_controller/joint_trajectory_controller

Two of them seem to be the "position_controllers/JointTrajectoryController".
Q: Can we somehow incorporate it with Moveit?

Thank you

[stefanscherzinger]

@Mohatashem

I succeeded in implementing the controllers on the real robot (UR10e mounted on a ceiling).

Great! I'm happy to hear that.

Q: How can this be avoided? Is there a calibration procedure, I need to run on ROS side? Is there anything else I am missing?

You might need to call a special service in the controller to signal that you were biasing the sensor. But I guess that needs some more refinement. See here and here and also here

Is it recommended stay within the range specified in the repo? Would it be safe to go lower than these values?

Yes, you can go lower by changing the config file and rebuilding the repositories.

Q: Does it mean that for cartesian_compliance_controller, we always need to set iterations = 1?

Not necessarily. But when you have a setting of controller gains that are at the edge of stability with your system, then increasing the iterations will make the system unstable. This is because the internal model simulates number of iterations before synchronizing its joint states with the real hardware. A higher number of iterations leads to bigger virtual steps, which is perceived as dead time in contacts. If you want iterations > 1, then also lower your controller gains a little.

Q: Is the above mentioned method correct/recommended?

It looks correct to me, yes. Building in Release mode is recommended, unless you are debugging.

Increasing the pd_gains, any further and the robot begins to oscillate slightly as soon as touched, but otherwise remains fluid and without oscillations, as soon as I let go. So I guess, for now that would be the "optimal" limit for me.

Note that stability can be lost per Cartesian axes. It's sometimes not obvious, which of the controller gains is the one causing instability. You might get a more responsive and stable system by tweaking the relation between individual axes a bit more. This, however, is task dependent.

3. Q: What is the utility of the following controllers in the yaml files?
   Q: Can we somehow incorporate it with Moveit?

Yes, this can be used with Moveit. I don't have a tutorial for that yet, but maybe this short info gives a direction.

[ZZWang21]

Hi @Mohatashem

That's ok to post it here.

1. What is the difference between the topics /target_wrench and /my_cartesian_force_controller/ft_sensor_wrench?

The /target_wrench is the desired wrench vector that your robot should apply to its environment via the specified end-effector. In terms of control theory, it's the reference signal. Closed-loop control needs to measure the actual contact forces to compute error correction. This is the ft_sensor_wrench, in control theory the controlled variable.

But I can’t seem to find the error_scale anywhere.

It's a little bit hidden. It's part of the solver namespace for each controller:

2. ... But I can't seem to find this topic.

You are right. Thanks for pointing this out. I guess I remapped both the cartesian_force_controller and the cartesian_compliance_controller to work with the /target_wrench in the example, so that's easier to play with them.

but what is the topic I should be publishing to, to simulate the actual case?

The /target_wrench is the right topic to publish to. I'll update the README.md. Note that this is sufficient for steering the robot around a little. If you want to simulate contacts (or once you work on the real UR10e), you need a sensor that publishes force-torque measurements on the respective ft_sensor_wrench topic.

3. For compliance controller ...

That's the same case. See 2..

4. ... But should we be worried about it?

No, you needn't worry. Ignore that for now. I need to rework that feature a little (#40 ).

---

Hi, you mentioned "you need a sensor that publishes force-torque measurements on the respective ft_sensor_wrench topic." In my case, I have a UR5e robot, one topic "/wrench" which outputs the measured force and torque (I cannot see a "force_sensor_link" in my case), should I publish this topic to "my_cartesian_force_controller/ft_sensor_wrench"? Since currently, the "my_cartesian_force_controller/ft_sensor_wrench" has nothing.

Thank you. @stefanscherzinger @Mohatashem

[Mohatashem]

Hello @ZZWang21,
Assuming you are using a real robot and rest of your setup is correct, you will need to remap your /wrench to /my_cartesian_force_controller/ft_sensor_wrench in your launch file.

<remap from="/wrench" to="/my_cartesian_force_controller/ft_sensor_wrench"/>

Alternatively, this also worked for me (directly from the command line terminal):

rosrun topic_tools relay wrench my_cartesian_force_controller/ft_sensor_wrench

All the best

[ZZWang21]

@Mohatashem Thank you for your reply. I have tried yesterday, it works.

[ZZWang21]

Hi @Mohatashem, I see, you have done your control on real UR10 robot. Is your yaml file just like what you post in this issue? So you have loaded all the controllers? But in that yaml file, I did not see "joint_to_cartesian_controller".

Do I have to load all the controllers to control the movement of the robot (in rqt or in Rviz or using python code)? Thank you.

[Mohatashem]

Hello,
The yaml I posted here was for a gazebo simulation. However, the real one was very similar to it (I was using namespaces differently, as we had more than one robot). You could remove the $(arg ceiling_prefix) everywhere and you could use it with the usual setup.

But in that yaml file, I did not see "joint_to_cartesian_controller".

Yes, that is because we didn't need it. If I've understood correctly, it's probably for using with Moveit but we didn't proceed in that direction.

Do I have to load all the controllers to control the movement of the robot (in rqt or in Rviz or using python code)

If I am not mistaken, you can load any and all controllers but start only the controller you need. For example, if I needed Cartesian control only I'd start only my_cartesian_motion_controller (along with my_motion_control_handle if you are following the tutorial, otherwise my_motion_control_handle must be off in case you are publishing to /target_frame from any other node), if you wanted only force control or hand-guiding motion, you could start the my_cartesian_force_controller and so on.

Hope it helps