setup questions with Univeral_Robots_ROS_Driver and cartesian_controllers

[objimp]

Hello. Apologies for the novice question, but I'm trying to follow the instructions to use the Cartesian Motion Controller with a physical UR10 robot and having a few setup issues, maybe something that can be clarified in the documentation?

Using the UniversalRobots/Universal_Robots_ROS_Driver , setup as per its readme, with downloaded kinematics calibration from the robot. Is completely functional with the scaled_pos_joint_traj_controller, can move the robot in rvis, plan and execute, as well as communicating over other ROS interfaces.

I've updated Universal_Robots_ROS_Driver/ur_robot_driver/config/ur10_controllers.yaml adding to the end of the controllers list

demo_cartesian_motion_controller:
type: position_controllers/CartesianMotionController
end_effector_link: tool0
robot_base_link: base_link
target_frame_topic: target_frame
joints: *robot_joints
pd_gains:
(all gains as per the example file)

as well as updating ur10_bringup_launch to add demo_cartesian_motion_controller to the list of controllers to start (and remove the scaled_pos_joint_traj_controller from that list because otherwise they conflict on joints)

I've also updated in the fmauch_universal_robot/ur10_moveit_config/config/controllers.yaml as per the example (except for renaming the joints to match the UR joint list. I have tried both adding as an additional controller and also as the only controller

When I try to launch roslaunch ur10_moveit_config ur10_moveit_planning_execution.launch, it errors that there is no action_ns for the CartesianMotionController so won't load. If I try to add action_ns: follow_joint_trajectory, it errors Unknown controller type: position_controllers/CartesianMotionController.

Hopefully this is some simple issue? Once this is working for me I'd be happy to help draft a step-by-step for bringing up a clean installation with UR robot.

Thanks.

[stefanscherzinger]

Hi @objimp

I'm trying to keep up with old issues. Thanks for describing your concerns with the setup. Did you manage to get it running in the meantime?

[objimp]

No, I gave up and just used moveit and haven't tried again recently.

[stefanscherzinger]

Alright. Thanks for the feedback. If you are still interested in getting this to run, please post the error messages that you are getting.

[zhanibekrysbek]

Hi Stefan,

Thanks for your contribution. From the presentation and papers I realize you did great work! Especially, such controllers does not come out of the box for Universal Robots.

I am struggling on setting up this package with real UR10e robot on ROS Noetic. It lacks documentation on how to use the code, how to define the robot descriptions and example scripts on using each control methods (Cartesian trajectory control, force control, and compliance controller). It would be great if you could provide more verbose documentation on this package, as I believe will benefit a lot of people, especially for those who are not familiar with ros_control like me.

As it comes to my issues, I setup UR10e successfully using UniversalRobots/Universal_Robots_ROS_Driver as @objimp. For my application, I need impedance/admittance control, so I stumbled upon on this repo, where cartesian_compliance_controller seem should do the work for me. But, I am stuck in these steps:

1. I don't know how to provide proper urdf files of UR10e. I see cartesian_controllers/cartesian_controller_examples/urdf/robot.urdf.xarco file that is based on UR10. However, Universal_Robot_ROS_Driver defines the robot in slightly different way, when I refer to the corresponding xarco files in ur_description/urdf/ur10e.xarco it shows an error. Could you refer to a tutorial that shows how to do this step properly or explain it here?
2. Now suppose, UR10e description file is setup properly, how it will be used together with UniversalRobots/Universal_Robots_ROS_Driver? Suppose, I launch ur10e_bringup command. And so what? How do I access controllers from cartesian_controllers metapackage?

I apologize for these beginner questions, as I couldn't find any other source, and do not know someone who has been through this.

Thanks!

[stefanscherzinger]

Thanks @zhanibekrysbek for your interest in this work!

It would be great if you could provide more verbose documentation on this package, as I believe will benefit a lot of people, especially for those who are not familiar with ros_control like me.

Yes, you are probably right. I was thinking of creating a more step-by-step introduction (#47), but didn't find much time lately.

I setup UR10e successfully using UniversalRobots/Universal_Robots_ROS_Driver as @objimp.

That's a good starting point. It's not too difficult from there. I'll try to summarize the main steps to take and cover your questions 1. and 2. on the way:

1. Build the cartesian_controllers in the ROS workspace that you will use to control your UR10e robot.

2. Start your UR10e according to their documentation.

3. When the robot is up and running, inspect its TF tree. You need to specify the kinematic chain from base to tip for the cartesian_compliance_controller, and a suitable way of finding good URDF links is by opening rqt -> Plugins -> Visualization -> TF Tree.

4. Create a new .yaml file for the cartesian_compliance_controller. You can use the entries from the examples and replace the links with actual links from the UR10e's TF tree.
   e.g.:

    end_effector_link: "tool0"    # All links below must be part of the same kinematic chain
    robot_base_link: "base_link"
    ft_sensor_ref_link: "tool0"
    compliance_ref_link: "tool0"
    target_frame_topic: "target_frame"
    joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
   

5. ROS-control's central component is the controller_manager. This thing manages all controllers at runtime. It will be able to load, start, stop and unload every ROS-control conform controller that it finds in it's namespace. In a sourced environment, find that namespace with calling

   rosservice list | grep controller_manager

   This will indicate where the controller_manager lives. It's probably in / (the root namespace), but you never know..

6. Once you have that namespace, load the .yaml file to the parameter server in that namespace. You can create a standalone .launch file to do this. Something like:

   <launch>
        <group ns="namespace-of-the-controller-manager">
   
           <!-- Load ROS controller configurations to parameter server -->
           <rosparam file="$(find your_package)/config/your_cartesian_compliance_controller.yaml" command="load"/>
       </group>
   </launch>

7. The next steps are easier the first time with rqt. You'll need the joint_trajectory_controller plugin. Install it with

   sudo apt-get install ros-noetic-rqt-joint-trajectory-controller

   In a source terminal, start rqt. If the controller_manager's namespace from above was not root, set that before starting rqt:

   export ROS_NAMESPACE=namespace-of-the-controller-manager

8. Inside rqt, navigate to Plugins -> Robot Tools -> Controller Manager. You should see several Controllers, some of them active (green), some of them stopped (red) and some of them unloaded (grey). Try loading and starting the cartesian_compliance_controller. Upon loading, the controller_manager instantiates the CartesianComplianceController class from the specification it sees on the parameter server. There's no need to provide a specific robot_description for the cartesian_controllers. They will search for the next best up their namespace. It will probably fail to start, because the joints are claimed by another controller. Check the warning message for the respective controller and stop that. Then try starting the cartesian_compliance_controller again. Carefully touch the robot's sensor and feel if that is compliant to some sort. Although not merged into master yet, try to work with the devel branch. That has an important change concerning noise-robustness.

Try these steps and let's continue from what you encounter.

[zhanibekrysbek]

I apologize for the delayed response. Thanks @stefanscherzinger for your prompt response and detailed explanation.

I followed these steps, but couldn't get the performance from the robot. Here is the summary of what I did, hopefully you can spot what I was doing wrong.

First two steps are straightforward. Only thing is, I switched to devel branch as you have suggested.

1. Build the cartesian_controllers in the ROS workspace that you will use to control your UR10e robot.
2. Start your UR10e according to their documentation.

For the step 3 and 4, I basically copied the file cartesian_controllers/cartesian_controller_examples/config/example_controllers.yaml and adjusted the link names properly. The only changes were the joint names and the sensor link ft_sensor_ref_link: "flange" . Everything else remained the same.

3. When the robot is up and running, inspect its TF tree. You need to specify the kinematic chain from base to tip for the cartesian_compliance_controller, and a suitable way of finding good URDF links is by opening rqt -> Plugins -> Visualization -> TF Tree.
4. Create a new .yaml file for the cartesian_compliance_controller. You can use the entries from the examples and replace the links with actual links from the UR10e's TF tree.
   e.g.:

After that, I created the launch file to upload the controller parameters to ROS parameter server. And confirmed that the changes have been reflected via $ rosparam get in the command line.

6. Once you have that namespace, load the .yaml file to the parameter server in that namespace. You can create a standalone .launch file to do this. Something like:

Also, 7. rqt-joint-trajectory-controller has been installed smoothly. In fact, it was working before this, and I was able to control the robot joint angles using this rqt plugin.

8. Inside rqt, navigate to Plugins -> Robot Tools -> Controller Manager. You should see several Controllers, some of them active (green), some of them stopped (red) and some of them unloaded (grey). Try loading and starting the cartesian_compliance_controller. Upon loading, the controller_manager instantiates the CartesianComplianceController class from the specification it sees on the parameter server. There's no need to provide a specific robot_description for the cartesian_controllers. They will search for the next best up their namespace. It will probably fail to start, because the joints are claimed by another controller. Check the warning message for the respective controller and stop that. Then try starting the cartesian_compliance_controller again. Carefully touch the robot's sensor and feel if that is compliant to some sort. Although not merged into master yet, try to work with the devel branch. That has an important change concerning noise-robustness.

Finally, I activated my_cartesian_compliance_controller after unloading scaled_pos_joint_traj_controller that had a resource conflict. Moreover, I ignored the warning message below, as you have suggested in #47 (issuecomment-1033979286). Then, I tried to move the UR10e's end effector from the sensor side to see its performance, but it remained stiff, i.e no effect.

[ INFO] [1646784630.151620827]: Failed to load /my_cartesian_compliance_controller/gravity from parameter server
[ INFO] [1646784630.152116194]: Failed to load /my_cartesian_compliance_controller/tool from parameter server

After several attempts of repeating steps 3-8, I couldn't get the compliant robot performance. I don't know why.

Does it affect installing real-time kernel as it was suggested here ?

Is there a launch file that I have to execute this controller? Or activating it from rqt is enough? Btw, I couldn't find any leverages to tweak compliance_controller similar to rqt-joint-trajectory-controller. Also, I tried to publish to /my_cartesian_compliance_controller/target_wrench x=5N, robot initially made sudden movement (actually vibration rather than movement) and remained stiff as in regular position control mode.

Please, let me know what you think. Feel free to ask anything for debugging purposes.

[stefanscherzinger]

@zhanibekrysbek

Sorry, the last weeks were very busy. Did you make progress in the mean time?

Does it affect installing real-time kernel as it was suggested here ?

No, that should not be the problem.

Is there a launch file that I have to execute this controller? Or activating it from rqt is enough?

Activating it in rqt is enough.

I have the feeling that the sensor's force-torque data is not properly connected to the controller's input topic.
In a terminal, could you

rostopic list | grep sensor

and post the output here?

[zhanibekrysbek]

@zhanibekrysbek

Sorry, the last weeks were very busy. Did you make progress in the mean time?

Does it affect installing real-time kernel as it was suggested here ?

No, that should not be the problem.

Is there a launch file that I have to execute this controller? Or activating it from rqt is enough?

Activating it in rqt is enough.

I have the feeling that the sensor's force-torque data is not properly connected to the controller's input topic. In a terminal, could you

rostopic list | grep sensor

and post the output here?

Thanks!

Here is the output.

~$ rostopic list | grep sensor
/my_cartesian_compliance_controller/ft_sensor_wrench

The topic is there, but there is no information coming in.

~$ rostopic hz /my_cartesian_compliance_controller/ft_sensor_wrench 
subscribed to [/my_cartesian_compliance_controller/ft_sensor_wrench]
no new messages
no new messages

On the other hand, /wrench topic is fine and has the proper force-torque data:

~$ rostopic hz /wrench
subscribed to [/wrench]
average rate: 499.832
	min: 0.002s max: 0.003s std dev: 0.00027s window: 498
average rate: 499.483
	min: 0.002s max: 0.004s std dev: 0.00028s window: 998

Also, here is a ros_graph, in case it might help to debug:

[stefanscherzinger]

@zhanibekrysbek

Alright, Thanks.

The topic is there, but there is no information coming in.

That's at least one of the issues. Now we need to remap the sensor topic so that it is connected to the driver's measurements. Probably something similar to

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

Some additional notes before using this on the real hardware:

1. Always hold the dead man switch of the teach pendant close when testing
2. Before switching on the controller, lower its error_scale a little. You have access to this parameter in rqt as described in this answer. Set it e.g. to 0.01 and slowly increase it once you have confirmed that everything behaves as expected.
3. To check if everything behaves as expected, carefully guide the robot via its end-effector. It should feel compliant, albeit moving very slowly. The responsiveness is proportional to error_scale. If you have the feeling that the robot moves in the wrong way (e.g. it moves against your pushing direction instead of with it), check that the TF frames are part of the same kinematic chain as explained here.

---

Edit: Please switch to the latest master branch.

[zhanibekrysbek]

Thanks @stefanscherzinger

Here are the steps I tried:

1. Switched to master branch. Compiled the workspace.

2. Remapped the /my_cartesian_compliance_controller/ft_sensor_wrench topic to /wrench. This line added to the same launch file that loaded controller yaml file.
   btw, how to verify that the controller is actually receiving the wrench messages?

3. Activated my_cartesian_compliance_controller via rqt controller manager.

Still, the robot does not respond.

To better debug this, I am attaching the tf tree of the robot. Also, below is the definition of the controller. Are the link names correctly referred according to tf tree?

my_cartesian_compliance_controller:
    type: "position_controllers/CartesianComplianceController"
    end_effector_link: "tool0"    # All links below must come before this link
    robot_base_link: "base_link"
    ft_sensor_ref_link: "flange"
    compliance_ref_link: "tool0"
    target_frame_topic: "target_frame"
    tool: ""
    gravity: 9.81
    joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint

    stiffness:  # w.r.t. compliance_ref_link
        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}

[stefanscherzinger]

@zhanibekrysbek

btw, how to verify that the controller is actually receiving the wrench messages?

You could check if your remapping worked by confirming that there is now only the /wrench topic left, and that rostopic list | grep sensor gives an empty result. When doing rostopic info /wrench, you should see that the ur_hardware_interface node both publishes (via the driver's sensor) and subscribes (via the compliance controller) to the topic. If that is not the case, have another look at remapping in ROS.

Are the link names correctly referred according to tf tree?

Yes, that looks good to me. A wrong configuration of the controller's kinematics may lead to incorrect responses, such as moving in the x-axis when pushing in the y-axis. But it should not stop the robot from moving altogether.

This suggests, that the sensor is not properly connected yet to the controller.

[zhanibekrysbek]

@stefanscherzinger

Yes, the sensor is still not connected to the controller.

I already attempted to remap several times in a launch file. I tried launching the remap before activating the my_cartesian_complience_controller in rqt, and after. The result is still the same. The end effector of the robot is not compliant at all.

<launch>

    <remap from="/my_cartesian_compliance_controller/ft_sensor_wrench" to="/wrench" />
    
     <group ns="/">
        <!-- Load ROS controller configurations to parameter server -->
        <rosparam file="$(find cartesian_controller_examples)/config/ur10e_example_controllers.yaml" command="load"/>
    </group>
    
</launch>

After the remap, the rostopic info shows below messages:

$ rostopic info /my_cartesian_compliance_controller/ft_sensor_wrench 
Type: geometry_msgs/WrenchStamped

Publishers: None

Subscribers: 
 * /ur_hardware_interface (http://nile:38555/)

~$ rostopic info /wrench
Type: geometry_msgs/WrenchStamped

Publishers: 
 * /ur_hardware_interface (http://nile:38555/)

Subscribers: 
 * /PlotJugglerListener_1648492138116872989 (http://localhost:45311/)

The remap effect is there: there is a /ur_hardware_interface node subscribed to /wrench, but /ft_sensor_wrench still exists, and /ur_hardware_interface is subscribed to it too.

I tried to follow the tutorial you provided for the rostopic remap. Am I doing it in a wrong way?

Also, does my_cartesian_compliance_controller need any other controller to run in background? If so, which one of them must be activated. For the list of available controllers, please refer to the attached image of the controller manager.

[zhanibekrysbek]

@stefanscherzinger

It seems like I found how the topic should be remapped! I put the remap line into a ur_10e_bringup.launch file, so that, remap command is sent before initiating the cartesian controllers... I believe the issue with the sensor topic is done.

However, after activating the compliance controller, the UR10e aggressively vibrated and went to a violation mode. All the motors were shut down in an emergency.

What could be a problem here? Wrong PD gains? Or the fact that the ft sensor is placed at its wrist and it has non-zero reading all the time? Or a noise issue?

PS. I am working on the master branch now. All of the controller parameters were the default ones.

thanks!

[stefanscherzinger]

@zhanibekrysbek

Alright. Now it's connected.

the UR10e aggressively vibrated and went to a violation mode.

That indicates instability by too aggressive controller gains. Try this:

Before switching on the controller, lower its error_scale a little. You have access to this parameter in rqt as described in #47 (comment). Set it e.g. to 0.01 and slowly increase it once you have confirmed that everything behaves as expected.

---

Edit: The error_scale is a convenient way of scaling all of the controller gains at the same time. Once you have a value that works for your use case, set that in the .yaml config of the controller (in the solver sub namespace):

    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}

    solver:
        error_scale: 0.1

[zhanibekrysbek]

@stefanscherzinger

I tried with a smaller error_scale=0.1. Perhaps, I should preload it to the controller properly. Will check this today.

Where I can refer to learn about the controller gains, error scale, stiffness, and other details for this repo?

I went through the papers shown on the home page. Are those models presented in your papers accurately represent what has been implemented in this repo? If so, I can figure out the meaning of each param.

[zhanibekrysbek]

@stefanscherzinger

Finally it worked! Thank you so much!

I used the same approach: started with error_scale=0.01 and then slowly increased where I observed faster response. Strange thing is, it was fine going into error_scale=0.1! In fact, when I even pre-loaded with 0.1 and activated the controller it was still okay. Manipulator was more or less at the same configuration as yesterday where it vibrated a lot. So, I am pondering why it was vibrating yesterday..

How can I find the optimal value of error_scale? Is there a topic where you publish actual and desired control signals to compare the performance of the controller similar to /scaled_joint_pos_controllers?

Also, as a safety feature, is it possible to automatically start with smaller error_scale, and then only increase to the desired value?

Another issue, as you previously noted, robot kinematics description is wrong. It moves to different direction compared to applied wrench. Any suggestions on how to fix it?

Yes, that looks good to me. A wrong configuration of the controller's kinematics may lead to incorrect responses, such as moving in the x-axis when pushing in the y-axis. But it should not stop the robot from moving altogether.

[stefanscherzinger]

@zhanibekrysbek

Great! I'm happy that it's running now.

Where I can refer to learn about the controller gains, error scale, stiffness, and other details for this repo?

A brief overview is here. And more details are given in this document (p. 74, figure 4.5). That doesn't include error_scale, though, which is a useful add-on for the implementation.

How can I find the optimal value of error_scale?

Unfortunately, that involves a bit of tweaking for your use case. I usually adjust that by bringing the robot into contact and increase the error_scale until I observe oscillations. It might also be helpful to adjust the relation between trans and rot gains to achieve better stability.

Is there a topic where you publish actual and desired control signals to compare the performance of the controller similar to /scaled_joint_pos_controllers?

No, I don't think so. But that's an interesting idea. You could open an issue for that if you like.

Also, as a safety feature, is it possible to automatically start with smaller error_scale, and then only increase to the desired value?

Yes, of course. Set your error_scale in the controller's .yaml for startup and then use the service interface for dynamic_reconfigure in your code/scripts. I personally like doing that from Python, which treats dynamic parameters conveniently with dictionaries.

It moves to different direction compared to applied wrench. Any suggestions on how to fix it?

I would check if flange really represents the coordinate frame of the sensor measurements. Especially its orientation. When working with the URe series, I normally use tool0 for that.

[zhanibekrysbek]

@stefanscherzinger

A brief overview is here. And more details are given in this document (p. 74, figure 4.5). That doesn't include error_scale, though, which is a useful add-on for the implementation.

Thanks! I saw both documents, before, now will study them in detail.

I would check if flange really represents the coordinate frame of the sensor measurements. Especially its orientation. When working with the URe series, I normally use tool0 for that.

I changed the sensor coordinate frame to tool0. Now, it perfectly responds to linear components of the applied wrench.
However, when it comes to rotation the robot is absolutely stiff no matter how I softened the stiffness coefficients. What could be the solution for this problem?

Here is how I provided the link names for the controller:

my_cartesian_compliance_controller:
    type: "position_controllers/CartesianComplianceController"
    end_effector_link: "tool0"    # All links below must come before this link
    robot_base_link: "base_link"
    ft_sensor_ref_link: "tool0"
    compliance_ref_link: "tool0"
    target_frame_topic: "target_frame"
    tool: ""
    gravity: 9.81
    joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint

• I tried the stiffness at the lowest value(100N/m). Even with that, robot's end effector is a bit heavy. Is there a way to make it even more compliant? I used to lower the damping params for a different robot. Do you provide an option to play with damping coefficient for the virtual spring-damper system? Is pd_gain supposed to change its response time?

• When a payload is mounted on the robot, do I have to change any kinematic/dynamic parameters for the controller? Usually, when the payload is mounted, robot end-effector position offsets down due to the constant weight of the payload instead of keeping the equilibrium at the same level.

[stefanscherzinger]

@zhanibekrysbek

However, when it comes to rotation the robot is absolutely stiff no matter how I softened the stiffness coefficients. What could be the solution for this problem?

That must be due to relatively low p gains for the rotational axes. Try increasing them by quite a bit (x10 or even x100). You can use the sliders in dynamic reconfigure for doing this (carefully and by slow steps).

I tried the stiffness at the lowest value(100N/m). Even with that, robot's end effector is a bit heavy. Is there a way to make it even more compliant? I used to lower the damping params for a different robot. Do you provide an option to play with damping coefficient for the virtual spring-damper system? Is pd_gain supposed to change its response time?

For this implementation, stiffness refers to the steady state stiffness, i.e. it generates the restoring forces to a displacement from the goal after reaching an equilibrium condition. Responsiveness is not determined by damping or mass (as in Impedance Control) but rather through the p gain for each axis. Try increasing and adjusting them individually. I would leave the damping gains d at zero for now.

When a payload is mounted on the robot, do I have to change any kinematic/dynamic parameters for the controller?

This might become necessary, especially in contact with the environment. If the payload has a relatively high mass (e.g. 10kg), then you probably need a lower error_scale, which scales all gains uniformly and decreases the system's overall responsiveness and hence its stability in contact.

Usually, when the payload is mounted, robot end-effector position offsets down due to the constant weight of the payload instead of keeping the equilibrium at the same level.

Yes, that's an effect that happens. My wish would be to provide a mechanism for at least gravity compensation, but that's work in progress (applies to both CartesianComplianceController and CartesianForceController.

[zhanibekrysbek]

@stefanscherzinger

Thanks a lot. We have much better understanding of the controller now.

Now, we are in the process of moving the target frame (or equilibrium pose x_d) and enforce the compliance along the trajectory. I guess the way to do so is to publish geometry_msgs/PoseStamped to /my_cartesian_compliance_controller/target_frame, right? Is there any requirements on the continuity of the target_frame?

We tried to provide static value for the target frame topic, but did not observe any movement from the robot. What could be the problem here? Is it because of the wrong topic subscription similar to wrench vs ft_sensor_wrench topic? (see below how it is specified in yaml file) Or it is due to stiffness values?

Here is how I provided the link names for the controller:

my_cartesian_compliance_controller:

type: "position_controllers/CartesianComplianceController"
    end_effector_link: "tool0"    # All links below must come before this link
    robot_base_link: "base_link"
    ft_sensor_ref_link: "tool0"
    compliance_ref_link: "tool0"
    target_frame_topic: "target_frame"
    tool: ""
    gravity: 9.81
    joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint

On the other hand, my_motion_control_handle for example, declared in the same file has a full topic specification for target frame as /my_cartesian_motion_controller/target_frame. Does it have to be the same for /my_cartesian_compliance_controller?

my_motion_control_handle:
   type: "cartesian_controllers/MotionControlHandle"
   end_effector_link: "tool0"
   robot_base_link: "base_link"
   target_frame_topic: "/my_cartesian_motion_controller/target_frame"
   joints:

Another question, is it possible to change the relative frame of the stiffness matrix in the dynamic reconfigure window? Per my observations, it is defined in tool0? frame Is it possible to affix it wrt base frame?

thanks again, I very much appreciate your time and help.

[zhanibekrysbek]

@stefanscherzinger

Update. We tried to publish PoseStamped message to target_frame topic continuously at 20Hz, with small deviation from the current cartesian position. But the robot still does not move. Also, we attempted to change the target frame topic name from target_frame to /my_cartesian_compliance_controller/target_frame in .yaml file.

We are receiving the following error:
[ WARN] [1649723744.336292514]: Got target pose in wrong reference frame. Expected: base_link but got

What it might indicate?

[stefanscherzinger]

@zhanibekrysbek Sorry for the late reply. I'm catching up after my vacation.

Are the issues still relevant for your?

I guess the way to do so is to publish geometry_msgs/PoseStamped to /my_cartesian_compliance_controller/target_frame, right?

Yes. Exactly. You can of course remap everything as you wish.

Is there any requirements on the continuity of the target_frame?

No, not really. It depends on your controller parameters. You can adjust the controller to follow smoothly (but delayed), for which 10 Hz (or even less would be ok). Or, the controller can be tweaked to be an almost exact IK-solver, for which a higher rate and a finer interpolation would be required.

Is it possible to affix it wrt base frame?

No, that's unfortunately not possible for now. But it might be a nice feature request. You could open an issue for that and I'll see how to implement that.

[ WARN] [1649723744.336292514]: Got target pose in wrong reference frame. Expected: base_link but got ...
What it might indicate?

Good, you are close. That's a safety check. It means that you need to fill the frame_id of the message with base_link. But be careful, that means that you promise that the values you give in that message are given in base_link coordinates and are safe to drive to with the end_effector_link.

[matthias-mayr]

For completeness, we have now open sourced our Cartesian impedance controller implementation, that complements this controller package for effort-controlled robots. This paper outlines the design & implementation considerations.

[shrutichakraborty]

@zhanibekrysbek

Alright, Thanks.

The topic is there, but there is no information coming in.

That's at least one of the issues. Now we need to remap the sensor topic so that it is connected to the driver's measurements. Probably something similar to

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

Some additional notes before using this on the real hardware:

1. Always hold the dead man switch of the teach pendant close when testing

2. Before switching on the controller, lower its `error_scale` a little. You have access to this parameter in rqt as described in [this answer](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/issues/47#issuecomment-1033979286). Set it e.g. to `0.01` and slowly increase it once you have confirmed that everything behaves as expected.

3. To check if everything behaves as expected, carefully guide the robot via its end-effector. It should feel compliant, albeit moving very slowly. The responsiveness is proportional to `error_scale`. If you have the feeling that the robot moves _in the wrong way_ (e.g. it moves against your pushing direction instead of with it), check that the TF frames are part of the same kinematic chain as explained [here](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/issues/32).

Edit: Please switch to the latest master branch.

Hi @stefanscherzinger @zhanibekrysbek , I have sucessfully installed these controllers for ROS2 and now doing the same for ROS1 setup with UR10e robot. I followed the steps above and still face an issue :
The controller fails to load the gravity and tool parameters as you see below :

[ INFO] [1713183634.120944050]: Forward dynamics solver initialized [ INFO] [1713183634.120981153]: Forward dynamics solver has control over 6 joints [ WARN] [1713183634.154574794]: Failed to load /cartesian_compliance_controller/target_frame_topic from parameter server. Will default to: /cartesian_compliance_controller/target_frame [ INFO] [1713183634.163057890]: Failed to load /cartesian_compliance_controller/gravity from parameter server [ INFO] [1713183634.163497537]: Failed to load /cartesian_compliance_controller/tool from parameter server

I have remapped the FT sensor topic with the following code in driver.xml file:
`

<remap from="cartesian_compliance_controller/target_wrench" to="target_wrench" />
<remap from="cartesian_compliance_controller/ft_sensor_wrench" to="wrench" />

`

I can confirm that the remaping has worked as I only have a /wrench topic and no longer have the /ft_sensor_wrench topic. However, one issue is that the frame that my /wrench topic publishes in is too0_controller whereas in the controller cnfig file I have :
cartesian_compliance_controller: type: "position_controllers/CartesianComplianceController" end_effector_link: tool0 robot_base_link: base_link ft_sensor_ref_link: tool0 compliance_ref_link: tool0

changing ft_sensor_ref_link: to tool0_controller results in an error :
[ERROR] [1713184071.310469055]: tool0_controller is not part of the kinematic chain from base_link to tool0

Please let me know where I am going wrong. Thanks!