• Control with MoveIt of Panda robot in Python

• 🗣️ CoMPaPy = [kɔm papi] 👴 (🇫🇷)

• 🚧 work in progress 👷

CoMPaPy

👓 description

"a simple python interface for basic control of a real panda robot with moveit"

• using only official and maintained repos: franka_ros and MoveIt
• two modes: simulated and real panda
• no cpp, just python

⚠️ limitations

• rely on ROS 😕
• only tested on the following combination
  • (ubuntu 20, noetic, libfranka 0.10.0, franka_ros 0.10.1)
  • todo: docker
• require switching to a real-time kernel to control the real panda
• require lots of installations
  • todo: docker
• only basic actions
  • move_j
  • move_l
  • open_gripper
  • close_gripper
  • rotate_joint
• may require some parameter tuning
  • e.g. compute_cartesian_path() params in move_l
• control panda_link8 instead of the gripper 😩
  • c.f. known issues
  • todo: ros.org/question/334902

🔧 installation

note: installing libfranka and franka_ros with sudo apt install ros-noetic- ... is not an option at the time of writing: it gives incompatible versions

create a directory for all packages

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src

🎯 final structure

click to expand

tree ~/catkin_ws/src -d -L 2

.
├── compapy
│     ├── config
│     ├── launch
│     ├── media
│     ├── logs
│     └── scripts
├── franka_ros
│     ├── cmake
│     ├── franka_control
│     ├── franka_description
│     ├── franka_example_controllers
│     ├── franka_gazebo
│     ├── franka_gripper
│     ├── franka_hw
│     ├── franka_msgs
│     ├── franka_ros
│     └── franka_visualization
├── geometric_shapes
│     ├── cmake
│     ├── include
│     ├── src
│     └── test
├── moveit
│     ├── moveit
│     ├── moveit_commander
│     ├── moveit_core
│     ├── moveit_experimental
│     ├── moveit_kinematics
│     ├── moveit_planners
│     ├── moveit_plugins
│     ├── moveit_ros
│     ├── moveit_runtime
│     └── moveit_setup_assistant
├── moveit_msgs
│     ├── action
│     ├── dox
│     ├── msg
│     └── srv
├── moveit_resources
│     ├── dual_panda_moveit_config
│     ├── fanuc_description
│     ├── fanuc_moveit_config
│     ├── moveit_resources
│     ├── panda_description
│     ├── panda_moveit_config
│     ├── pr2_description
│     ├── prbt_ikfast_manipulator_plugin
│     ├── prbt_moveit_config
│     ├── prbt_pg70_support
│     └── prbt_support
├── moveit_tutorials
│     ├── doc
│     ├── _scripts
│     ├── _static
│     └── _themes
├── moveit_visual_tools
│     ├── include
│     ├── launch
│     ├── resources
│     └── src
├── panda_moveit_config
│     ├── config
│     └── launch
├── rviz_visual_tools
│     ├── icons
│     ├── include
│     ├── launch
│     ├── resources
│     ├── src
│     └── tests
├── srdfdom
│     ├── include
│     ├── scripts
│     ├── src
│     └── test
└── ven
    ├── bin
    ├── include
    ├── lib
    ├── lib64 -> lib
    └── share

⛏️ building from source

only tested with these versions (source):

Robot system version	libfranka version	franka_ros version	Ubuntu / ROS
> = 5.2.0 (FR3)	> = 0.10.0	> = 0.10.0	20.04 / noetic

🔍 libfranka

follow these instructions

📦 ROS meta-packages

🐼 franka_ros

follow these instructions

• prefer catkin build to catkin_make

⏳ real-time kernel

follow these instructions

• check with uname -r
• note: the real-time kernel seems to be required to use the real robot, but not for gazebo / rviz simulations

🤸 moveit_panda_config

follow these instructions

• prefer ~/catkin_ws/src to ~/ws_moveit/src

🤸 compapy

cd ~/catkin_ws/src
git clone https://github.com/chauvinSimon/compapy.git
cd ~/catkin_ws/
catkin build

🎭 venv

cd ~/catkin_ws/src
python3 -m venv ven --system-site-packages
source ven/bin/activate
pip install -r requirements.txt

✏️ source_catkin alias in ~/.bashrc

this may be useful

alias source_catkin="source ~/catkin_ws/devel/setup.bash; source ~/catkin_ws/src/ven/bin/activate; cd ~/catkin_ws/src"

🐛 troubleshooting

"Unable to find either executable 'empy' or Python module 'em'... try installing the package 'python-empy'"

• (ven) ~/catkin_ws$ catkin build -DPYTHON_EXECUTABLE=/usr/bin/python3 -DPYTHON_INCLUDE_DIR=/usr/include/python3.8

✍️ pycharm

follow this great video (Peter Mitrano)

source ~/catkin_ws/src/ven/bin/activate
python -c "import ros; print(ros.__file__)"

# if not using the alias
source ~/catkin_ws/devel/setup.sh

find the path to pycharm.sh either with JetBrains ToolBox or with

sudo apt install locate
sudo updatedb
locate pycharm.sh

I prefer to run python scripts from pycharm 😊

• Working directory is configured to be ~/catkin_ws/src/compapy
• if you prefer using the terminal, you may need to export PYTHONPATH=${PYTHONPATH}:${PWD} against ModuleNotFoundError and ModuleNotFoundError

▶️ starting real robot

⚠️ keep the emergency button close to you ⛑️

the first time: follow the fci instructions to configure fci

switch on the robot

in http://172.16.0.2/desk/

• release joints
• activate fci
• Re-initialize Hand

in a terminal

• source_catkin (the alias defined in a previous section)

🩺 verifications

👋 manual control

• move the arm

  • in programming mode
  • see this 30-sec demo

• open/close the gripper

  • in programming mode or execution mode
  • see this 15-sec demo
  • FCI can stay activated but make sure to close RViz and stop all ROS nodes (otherwise End Effector: Not Connected)

✔️ expected result

☎️ robot connection

ping 172.16.0.2

🎮 move group python Interface

no real robot needed: follow this tutorial

roslaunch panda_moveit_config demo.launch rviz_tutorial:=true
rosrun moveit_tutorials move_group_python_interface_tutorial.py

✔️ expected result

📖 libfranka

in execution mode

~/libfranka/build/examples/communication_test 172.16.0.2

✔️ expected result

🍿 franka_example_controllers

in execution mode

roslaunch franka_visualization franka_visualization.launch robot_ip:=172.16.0.2 load_gripper:=true

# move a bit the gripper from its start pose before running this
roslaunch franka_example_controllers move_to_start.launch robot_ip:=172.16.0.2

notes

• the pose defined in start_pose.yaml is reached

  • the max_dq parameter, in rad/s, is used to control the duration of the move, hence the speed

• franka_visualization.launch raises the following, but the command can still be executed

  • Robot semantic description not found. Did you forget to define or remap '/robot_description_semantic'?

📺 rviz visualization

in programming mode

roslaunch franka_visualization franka_visualization.launch robot_ip:=172.16.0.2 load_gripper:=true

• move the arm manually (gently press the two buttons on the gripper)
• see the corresponding motion in rviz

note

franka_visualization.launch raises the following, but motion in rviz can still be seen

Robot semantic description not found. Did you forget to define or remap '/robot_description_semantic'?

🖱️ rviz control

in execution mode

roslaunch panda_moveit_config franka_control.launch robot_ip:=172.16.0.2 load_gripper:=true

in rviz:

• add the MotionPlanning plugin
• in Planning Request, enable Query Goal State
• drag the interactive marker to some the goal state
• click Plan and the Execute
• try different planning settings (cartesian path, speed, planning pipeline ...)
• try to display and hide the different visualisation tools
• I personally like the Joints tab in MotionPlanning plugin

🗜️ gripper

in execution mode

run one of

roslaunch franka_gripper franka_gripper.launch robot_ip:=172.16.0.2
roslaunch franka_control franka_control.launch robot_ip:=172.16.0.2
roslaunch panda_moveit_config franka_control.launch robot_ip:=172.16.0.2 load_gripper:=true
roslaunch compapy real.launch robot_ip:=172.16.0.2

alternatively to the real robot, gazebo can be used

roslaunch panda_moveit_config demo_gazebo.launch

# close the gripper
rostopic pub --once /franka_gripper/grasp/goal franka_gripper/GraspActionGoal "goal: { width: 0.022, epsilon:{ inner: 0.005, outer: 0.005 }, speed: 0.1, force: 5.0}"

# open the gripper
rostopic pub --once /franka_gripper/move/goal franka_gripper/MoveActionGoal "goal: { width: 0.08, speed: 0.1 }"

✔️ expected result

🦿 joints

in execution mode

roslaunch compapy real.launch robot_ip:=172.16.0.2

install and run rqt_joint_trajectory_controller

sudo apt install ros-noetic-rqt-joint-trajectory-controller
rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller

in the two drop down options select /controller_manager and effort_joint_trajectory_contoller and then press the red button to "enable sending commands to the controller"

use this rqt tool:

• in execution mode to move each joint individually (done with gazebo in the next section)

• in programming mode or in a simulation to monitor the value of each joint and the distance to joint limits

✔️ expected result

🎭 gazebo

🍽️ ground_plane

about ground_plane

• the default world contains a ground_plane
• it acts as an obstacle and prevents the robot from reaching negative z
• it is located in /usr/share/gazebo-11/worlds/empty.world

two solutions

• either ... delete the ground_plane in Models
• or ... create a my_empty.world with just a sun, and add the args world:=[ABSOLUTE_PATH_TO_my_empty.world] to the demo_gazebo.launch

<?xml version="1.0" ?>
<sdf version="1.5">
    <world name="default">
        <!-- A global light source -->
        <include>
            <uri>model://sun</uri>
        </include>
    </world>
</sdf>

roslaunch panda_moveit_config demo_gazebo.launch [world:=/home/simonchauvin/catkin_ws/src/compapy/config/my_empty.world]

rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller

✔️ expected result

📏 read cartesian coordinates

run one of

roslaunch panda_moveit_config franka_control.launch robot_ip:=172.16.0.2 load_gripper:=true
roslaunch compapy real.launch robot_ip:=172.16.0.2

alternatively to the real robot, gazebo can be used

roslaunch panda_moveit_config demo_gazebo.launch

then

• in rviz add tf under Panels
• under Frames, panda_EE gives the current transform from the base (world) to the end effector
• move the robot
  • either manually in programming mode
  • or by commands in execution mode
• read the updated transform

alternatively, without rviz:

rosrun tf tf_echo /panda_link0 /panda_EE

✔️ expected result

🚟 CoMPaPy control

in execution mode

roslaunch compapy real.launch robot_ip:=172.16.0.2

alternatively to the real robot, gazebo can be used

roslaunch panda_moveit_config demo_gazebo.launch

in pycharm, with Working directory set to ~/catkin_ws/src/compapy

python scripts/tests/test_ref_actions.py

✔️ expected result

unit-test with 5-dof

unit-test with 7-dof

🏁 usage

🥢 gripper

adjust the parameters for close_gripper(), depending on the width of the object to grasp, in compapy.yaml

🌲 scene

define obstacles in obstacles.json

then visualize them with

roslaunch compapy sim.launch

in pycharm, with Working directory set to ~/catkin_ws/src/compapy

python scripts/main_load_obstacles.py

✔️ expected result

🕹️ example

in execution mode

roslaunch compapy real.launch robot_ip:=172.16.0.2

adapt and run main_example.py

in pycharm, with Working directory set to ~/catkin_ws/src/compapy

python scripts/main_example.py

🐛 troubleshooting

calling a compapy function that moves the arm, while in programming mode

[ INFO] [1674462487.629393072]: ABORTED: CONTROL_FAILED
[ WARN] [1674462487.628997406]: Controller 'position_joint_trajectory_controller' failed with error GOAL_TOLERANCE_VIOLATED: panda_joint1 goal error -0.560709
[ WARN] [1674462487.629082297]: Controller handle position_joint_trajectory_controller reports status ABORTED
ERROR - 2023-01-23 09:28:07,629 - CoMPaPy: failed to execute plan
ERROR - 2023-01-23 09:28:07,629 - CoMPaPy: execution of plan failed
ERROR - 2023-01-23 09:28:08,654 - move_to_start_and_set_limits: move failed: execution of plan failed

starting a script to control the amr (e.g. main_example.py), while no ROS is running

[ERROR] [1673445824.345144228]: [registerPublisher] Failed to contact master at [localhost:11311]. Retrying...
# note: `roslaunch` starts `roscore` (and therefore the term "`ROS` master")

running a .launch file, while ROS is already running

[ WARN] [1674459968.109411334]: Shutdown request received.
[ WARN] [1674459968.110716175]: Reason given for shutdown: [[/robot_state_publisher] Reason: new node registered with same name]

🤔 known issues

💯 [move_l] cannot compute an entire [plan]

what can help:

• check this answer

  • "A Cartesian path fraction less than 100% usually means either a collision was detected (which seems not to be the case here) or IK failed."

• change the eef_step and jump_threshold params of compute_cartesian_path(), e.g. several trials with random offsets
  • I observe fraction improvements of max. 1%, i.e. so not very helpful
• add intermediate waypoints to waypoints to make sub-paths easier
• change the orientation of the target_pose
  • I noticed that simple combinations of [straight lines] and [rotation around z of the gripper of rz deg] fail for certain rz values
• try move_j if this is an option 🤷‍♂️

🥴 the computed [plan] includes strange joint moves

left: joint_1 rotates first cw and then ccw, causing warnings or errors during the execution of the trajectory. right: joint_1 keeps rotating in only one direction

what can help:

• check this answer
• use a different planner in MoveIt (usually done with the pipeline argument)
• play with the jump_threshold param of compute_cartesian_path()
• reduce the degrees of freedom (dof) of the panda arm
  • ideally, create a new move_group using the MoveIt setup assistant where not all joints are used
    • e.g. freeze joint_3 and joint_5
    • todo: I could not find how to do it
  • alternatively, but not optimal, drastically reduce the bounds of joint_3 and joint_5 in joint_limits.yaml (note that fr3 is more constrained than panda)
    • moveit uses the limits defined in ~/catkin_ws/src/franka_ros/franka_description/robots/panda/joint_limits.yaml
    • ⚠️ after adjusting joint_limits.yaml, make sure all joints are in their intervals before starting moveit in the execution mode! Otherwise, the arm can strongly vibrate
    • in execution mode, run python scripts/move_to_start_and_set_limits.py --dof=5, which does the following:
      • clear changes in joint_limits.yaml, i.e. reset to 7-dof
      • move the amr to a pose suitable for 5-dof: joint_5 = joint_3 = 0.0
        • note: this can also be achieved using move_to_start.launch, or even manually:
          • in programming mode, with roslaunch compapy real.launch robot_ip:=172.16.0.2, manually move the amr so that joint_3 and joint_5 are at 0 (align the arrows printed on the joints)
          • make sure these two joints are at 0 with rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller
          • manually move the other joints roughly to the middle of their ranges
          • quit rviz and kill real.launch
      • overwrite joint_limits.yaml for 5-dof: reduce the bounds of joint_3 and joint_5 to ~0

🎲 [compute_cartesian_path] is not deterministic

using the same configurations and parameters, compute_cartesian_path() can return plan that differ in size

• despite setting python and numpy random.seed()
• probably a cpp seed setting is required instead

🦾 the [compapy.move] functions move the [panda_link8] frame to the passed [target_pose], not [panda_EE]

compapy.move functions move the panda_link8 frame to the passed target_pose, not panda_EE

• an example of frame conversion can be found in frame_conversion.py
• I could not find how to directly control panda_EE instead

here the measurements used to derive some of the parameters for the conversion:

measurements when the fingers are in contact with the ground plane: panda_EE.z = 8.5mm and panda_link8.z = 111.6mm

conclusions from the above measurements:

• the z offset between panda_EE and panda_link8 is ~103mm
• the origin of the panda_EE frame is located between the finger pads, at ~8mm from the tip

origin of panda_EE, at ~8.5mm from the tip

👍 acknowledgements

the following resources helped me understand how to control the robot

• Panda Programming Guide (Ahmad Al Attar)
• franka_ros_interface (Saif Sidhik)
• DE3-Panda-Wall (Keith Li, Daniel Yin, Zachary Yamaoka)
• panda-gazebo (Rick Staa)
• frankx (Berscheid)
• moveit_python (Michael Ferguson)