epfl-lasa / ridgeback_ur5_controller

Several controllers to move Ridgeback mobile-robot and UR5 robotic-arm.

Ridgeback+Ur5 Controller [DEVEL]

This repository provides several contollers for the Ridgeback mobile-robot with Ur5 robotic-arm.

• admittance_control: This package implements an admittance controller on the ridgeback+UR5 platform (see below for the control architecture).
• ur5_cartesian_velocity_control: This package provides a cartesian velocity controller (ros control) for the UR5 arm.
• obstacle_avoidance: This package provides a simple obstacle-avoidance for the platform. It looks for the nearest obstacle using the laser sensors and remove the velocity components in that direction.
• cpr_bringup: This package provides a series of launch files and ROS settings in order to start-up the real-robot as well as the simulator.
• cpr_mocap_tracking: This package enables the robot to track an object (using mocap system) in its own frame of references (automatic calibration is included).
• cartesian_state_msgs: It contains the defintion of message type "PoseTwist" (combination of the standard ros/geometry_msgs pose and twist).

---

Compliation and build

Clone the repository intor your catkin source directory

$ cd ~/catkin_ws/src
$ git clone [email protected]:epfl-lasa/ridgeback_ur5_controller.git

Get the source dependencies using wstool

$ wstool init
$ wstool merge ridgeback_ur5_controller/dependencies.rosinstall
$ wstool up

Get the package dependencies using rosdep

$ rosdep install -y --from-paths src --ignore-src --rosdistro indigo

You also need the following ros packages

$ sudo apt-get install ros-indigo-ridgeback-*
$ sudo apt-get install ros-indigo-universal-robot

if you are getting error for broken packages (most probably due to a wrong version of gazebo), you can use 'aptitude' instead of 'apt-get' which propose a solution and resolve the conflict.

Finally complie

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ catkin_make

• you might need the source the bash file and compie again if the first compliation could not find some of in house dependencies.

For simulator, you can use gazebo7

$ sudo apt-get install ros-indigo-gazebo7-*

You might need to follow these instructions.

---

---

Running the controller

To bring up the robot in simulation run

roslaunch cpr_bringup cpr_bringup.launch
roslaunch admittance_control admittance_controller.launch

For the real robot launch on the CPR main PC run

roslaunch cpr_bringup cpr_bringup.launch sim:=false
roslaunch admittance_control admittance_controller_real.launch

Expected behaviors

The behavior of different components are demonstrated here:

• admittance control and its rviz
• obstacle avoidance and its rviz.
• object tracking and its rviz.

Control Architecture

Kinematics and transformations

Here is a short list of important frames and their usage.

frame id	Usage
world	Odometry and navigation
ur5_arm_base_link	Arm pose and twist
base_link	Platform pose and twist
robotiq_force_torque_frame_id	External force applied to the end-effector

Adamittance dynamics

The following figure shows the controller architecture for the admittance control on the robot.

The two equations in the center describe the admittance dynamics which compute the desired accelaration for the arm and the platform. These accelerations are integrated in time to acheive the desired velocities for the robot. The low-level velocity controller fullfills these velocities. In the case of platform, the computed velocities can be modified accroding to obstacle avoidance node.

The admittance parameters (shown in purple) are as follows:

Variable	Parameter
Ma	Desired mass of the arm
Da	Desired damping of the arm
Dc	Desired damping of the coupling
Kc	Desired Stiffness of the coupling
Mp	Desired mass of the platform
Dp	Desired damping of the platform

These parameters are load from a yaml through the launch file.

External force

The external is initially measured by the force/torque sensor in its own frame reference. In admittance controller this force is transformed to "ur5_arm_base_link" where the control of arm takes place. To avoid reacting to small forces a deadzone is considered. Moreover, a low-pass filter is used to smooth the measurements. The parameters of the deadzone and the filter can be set from the launch file.

Higher-level control (Motion planning )

Through a higher level controller, the position of the equilibrium can be can be changed to acheive a desired behavior. Also, F_c can be used for control purposes; e.g., to compensate for the mass of an object carried by the arm.

ridgeback_ur5_controller's Issues

Admittance controller node dies

Hello,

I am trying to run the cpr robot with the admittance controller as instructed in the README.md. When I run the admittance controller launch file, the node dies because it can't retrieve the necessary parameters from the ROS server (e.g. "topic_arm_state"). Is there some kind of YAML file missing?

Thanks in advance.

Some Problem for /ur5/ur5_cartesian_velocity_controller/command_cart_vel Topic

I have run it and send some data for this topic but My UR5 robot has not move as published data.
How I can solve it?
Thanks.

Error when catkin_make

Hi, I got an error

"fatal error: cartesian_state_msgs/PoseTwist.h: No such file or directory
11 | #include "cartesian_state_msgs/PoseTwist.h"
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [Compliant-Control-and-Application/control_strategy/CMakeFiles/control_strategy.dir/build.make:63: Compliant-Control-and-Application/control_strategy/CMakeFiles/control_strategy.dir/src/control_strategy/control_strategy.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:9283: Compliant-Control-and-Application/control_strategy/CMakeFiles/control_strategy.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
In file included from /home/walter/catkin_ws/src/Compliant-Control-and-Application/control_algorithms/admittance/src/Admittance/Admittance.cpp:8:
/home/walter/catkin_ws/src/Compliant-Control-and-Application/control_algorithms/admittance/include/admittance/Admittance.h:14:10: fatal error: cartesian_state_msgs/PoseTwist.h: No such file or directory
14 | #include "cartesian_state_msgs/PoseTwist.h"
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [Compliant-Control-and-Application/control_algorithms/admittance/CMakeFiles/admittance.dir/build.make:63: Compliant-Control-and-Application/control_algorithms/admittance/CMakeFiles/admittance.dir/src/Admittance/Admittance.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:11586: Compliant-Control-and-Application/control_algorithms/admittance/CMakeFiles/admittance.dir/all] Error 2
In file included from /home/walter/catkin_ws/src/Compliant-Control-and-Application/control_algorithms/admittance/src/HybridAdmittance/HybridAdmittance.cpp:1:
/home/walter/catkin_ws/src/Compliant-Control-and-Application/control_algorithms/admittance/include/hybridadmittance/HybridAdmittance.h:6:10: fatal error: cartesian_state_msgs/PoseTwist.h: No such file or directory
6 | #include "cartesian_state_msgs/PoseTwist.h"
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
"

what is the problem? Thanks.

Inquire for robotiq_force_torque_sensor pkg

When I run this project in real robot arm(UR5), there are something wrong in the package dependeices and the error like this:
ERROR: cannot launch node of type [robotiq_force_torque_sensor/rq_sensor]: robotiq_force_torque_sensor
Would you like to help me with this?
Thanks.

Would you please provide further information about your admittance controller?

Hi,

1. Do you have any publications about this project?

I want to implement an admittance controller for dual-arm (two UR5 arms) coordinated operation.
Would you please provide more information about your admittance controller and cartesian_velocity_controller?

2. When running the simulation roslaunch cpr_bringup cpr_bringup.launch, I met with the error:
   Coudn't find transforms!,
   would you please give me some tips to fix it? It seems that I need to install ros_falcon, is it this one?

3. When I set sim = false, another errors come out:

[ERROR] [1553151516.720913705]: Could not find parameter robot_description on parameter server
[ INFO] [1553151516.722473671]: Setting prefix to ur5_arm_
[ INFO] [1553151516.722878423]: Reading rosparams from namespace: /ur5
[ INFO] [1553151516.723345986]: Loaded ur_hardware_interface.
[ur5/robot_state_publisher-7] process has died [pid 7418, exit code -11, cmd /opt/ros/indigo/lib/robot_state_publisher/robot_state_publisher __name:=robot_state_publisher __log:=/home/chunting/.ros/log/bf8f9c28-4ba6-11e9-be96-8cec4ba87d89/ur5-robot_state_publisher-7.log].
log file: /home/chunting/.ros/log/bf8f9c28-4ba6-11e9-be96-8cec4ba87d89/ur5-robot_state_publisher-7*.log
Loaded joint_state_controller
--------------------> name_space: /ur5/ur5_cartesian_velocity_controller
[ERROR] [1553151517.646763972]: KinematicChainControllerBase: No robot description (URDF)found on parameter server (/ur5/ur5_cartesian_velocity_controller/robot_description)

My environment is Ubuntu 14.04+ROS Indigo+UR5 CB 3.8.

Thanks for your kind assistance.
Chunting

admittance control issue

@jrmout

Here is the the problem that I have:
run the simulator (with fake or falcon wrenches turned off) and the admittance control .
and use rosrun rqt_ez_publisher rqt_ez_publisher to publish control forces on /wrench_control which will be read by the admittance control as u_c_

also watch the velocity; e.g., with rqt_plot for ```/ur5_cartiesian_velocity_controller_sim/cart_vel_world/linear/x and y and z.

apply 0.5 newton in different directions. For me it results in different velocity. My issue is that it is really hard to make the robot move in some direction, and a simple F=-D(v - v_traget) would not work.

Dependencies

I am doing this:

wstool init
wstool merge https://raw.github.com/ros-controls/ros_control/kinetic-devel/ros_control.rosinstall
wstool update 
cd ..
rosdep install --from-paths . --ignore-src --rosdistro kinetic -y

and I am getting

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
cpr_bringup: Cannot locate rosdep definition for [robotiq_modbus_tcp]