rickstaa/real-panda-control-examples

Control the Panda robot using a laptop

rickstaa opened this issue · 1 comments

rickstaa commented

This issue serves as a quick tutorial on working with the panda robot. For more information, see the official panda documentation.

Setup instructions

For the instructions on setting up the Franka Control Interface on the workstation see the Franka documentation. If you have problems with building the real-time kernel see this issue on the Libfranka repository.

Control examples

Below you will find several control examples that get you familiar on how to work with the robot.

Control the robot thro

Let's first try controlling the robot with the examples given in the franka_ros documentation.ugh franka_ros

Libfranka and franka_ros build instructions

When installing the libfranka and franka_ros according to the panda documentation make sure you build it from source. When building the franka_ros package you have to point catkin to the path where you compiled the libfranka library -DFranka_DIR:PATH=~/libfranka/build. Further, you have to make sure you provide the -DCMAKE_BUILD_TYPE=Release cmake argument.

Use the franka_gripper node

Start the franka_gripper node with the following command:

roslaunch franka_gripper franka_gripper.launch robot_ip:=172.16.0.2

We can then check the joint_states using:

rostopic echo /franka_gripper/joint_states

We can then call any of the spawned action services to control the robot. To see which action services are available use the rostopic list command. All the topics which contain a /cancel, /feedback, /goal, /result and /status suffixes are action services. We can call these action services by publishing to the goal topic directly, using a python or C++ script or by using the actionlib axclient.py tool (see the docs). We can for example home the gripper using the following command:

 rostopic pub /franka_gripper/move/goal franka_gripper/MoveActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  width: 0.04
  speed: 0.1"

The right message can be found by using bash tab autocompletion. Alternatively, we can use the axclient.py tool:

sudo apt install ros-noetic-actionlib-tools
rosrun actionlib_tools axclient.py /franka_gripper/move

Use the franka_visualization node

We can use the roslaunch franka_visualization franka_visualization.launch robot_ip:=172.16.0.2 load_gripper:=true command to show the robot state in rviz.

Use the franka_control node

Now let's try to control the whole robot using the franka_control package. For this, we can use any of the example launch files in the franka_example_controllers package. It contains the following examples:

  • cartesian_pose_example_controller.launch: Performs a carteasian pose control sequence using the libfranka FrankaPoseCartesianInterface interface.
  • cartesian_velocity_example_controller.launch: Performs a carteasian velocity control sequence using the libfranka FrankaVelocityCartesianInterface interface.
    • elbow_example_controller.launch: Will move the elbow while keeping the gripper still.
  • force_example_controller.launch: In this example, you can set the force the robot has to apply on a horizontal surface.
  • cartesian_impedance_example_controller.launch: Will implement an impedance controller that makes the robot go back to a certain cartesian pose with a given stiffness.
  • joint_position_example_controller.launch: Performs a join position control sequence using the libfranka PositionJointInterface interface.
  • joint_velocity_example_controller.launch: Performs a join velocity control sequence using the libfranka VelocityJointInterface interface.
  • model_example_controller.launch: Prints the full franka state using the libfranka FrankaStateInterface interface.

For more information on all the interfaces available in the libfranka library see the documentation. If you get an error during this examples you can reset the control using the following command:

rostopic pub -1 /franka_control/error_recovery/goal franka_msgs/ErrorRecoveryActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal: {}"

For several examples, the robot needs to start in the home position. The robot can be moved to this position with the move_to_start.launch launch file.

Example launch command
roslaunch franka_example_controllers move_to_start.launch robot_ip:=172.16.0.2 load_gripper:=true

Use moveit

We can also make use of Moveit to control the Franka robot through the franka_control package.

Use a desktop for the FCI workstation

When running the real-time kernel most desktops are suited for controlling the Franka robot. You can check if your desktop is suited by performing the network tests found in the panda_documentation.

How to control

If your desktop is suited you can control it directly on the desktop. If you want to use your laptop you have to make sure that your laptop is on the same network as the FCI workstation and control unit. If this is done you can both use the C++ libfranka API directly or use the ros_control package. Below you will find a quick example for controlling the panda robot via your laptop while having the desktop serve as an FCI workstation.

Use a laptop for the FCI workstation

I performed several tests to see if I could use my laptop as the FCI workstation to performing real-time control. In the end, after performing some tests and asking advice from Gijs van der Hoorn I came to the conclusion that my laptop is not suited for this purpose. To quote Gijs:

Laptops are not very well suited for running workloads with determinism requirements in general, simply because they tend to be designed for good mobility, which means lots of power management and other tricks to flexibly assign resources to where they are needed. Both at the OS level, as well as the firmware/hardware level.

If mobility is a requirement we are better of buying a SBC for performing the real-time control. The SBC for the FCI doesn't have very high processing requirements. What it needs
are a low-latency, low-jitter connection, with deterministic performance. Additionally, we can buy a second SBC for performing other calculations like RL network inference.

In our current setup, we now use a desktop to run the FCI (high-freq control) while connecting to it with a laptop to send the high-level control commands (low-freq control).

Performed tests

System info

I performed all the tests below on the following system:

OS: Ubuntu 20.04
Default kernel: 5.8.0-53
PC: HP Zbook G5 Studio x360
RAM: 2x32 GB Samsung M471A4G43MB1-CTD geheugenmodule 32 GB DDR4 2666 MHz
GPU: Nvidia Quadro P1000
CPU: Intel(R) Core(TM) i7-8750H CPU @ 2.20GHz

Network and performance tests

To see if the robot could be controlled in real-time from my laptop, I performed the network tests that are given in the panda documentation.

Simple ping test
[1621600461.833791] 1208 bytes from 172.16.0.2: icmp_seq=10000 ttl=64 time=0.480 ms

--- 172.16.0.2 ping statistics ---
10000 packets transmitted, 10000 received, 0% packet loss, time 9999ms
rtt min/avg/max/mdev = 0.183/0.512/0.773/0.072 ms

From the results above, we can see that the mean round-trip time is 0.512ms, and the max round-trip time is 0.773ms. Judging from the fact that the documentation achieves a max round-trip time of 0.5ms and states that the total control time has to be under 1ms, it might be that my network card in my laptop is too slow for real-time control. Let's perform the libfranka/build/examples/communication_test to see if this is the case.

Communication test

#9900 Current success rate: 0.59

Finished test, shutting down example.

#######################################################
The control loop did not get executed 73 times in the
last 10000 milliseconds! (lost 73 robot states)

Control command success rate of 9927 samples: 
Max: 1.00
Avg: 0.49
Min: 0.28

WARNING: THIS SETUP IS PROBABLY NOT SUFFICIENT FOR FCI!
PLEASE TRY OUT A DIFFERENT PC / NIC
#######################################################

My laptop is probably not sufficient enough to serve as a real-time control interface from the results above.

rickstaa commented

I continued my discussion on the discussions page.