`CoMPaPy`

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.5`mm 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)

Name		Name	Last commit message	Last commit date
Latest commit History 159 Commits
config		config
launch		launch
media		media
scripts		scripts
.gitignore		.gitignore
CMakeLists.txt		CMakeLists.txt
LICENSE		LICENSE
README.md		README.md
package.xml		package.xml
requirements.txt		requirements.txt

License

chauvinSimon/compapy

Folders and files

Latest commit

History

Repository files navigation

CoMPaPy

👓 description

⚠️ limitations

🔧 installation

🎯 final structure

⛏️ building from source

🔍 libfranka

📦 ROS meta-packages

🐼 franka_ros

⏳ real-time kernel

🤸 moveit_panda_config

🤸 compapy

🎭 venv

✏️ source_catkin alias in ~/.bashrc

🐛 troubleshooting

✍️ pycharm

▶️ starting real robot

🩺 verifications

👋 manual control

☎️ robot connection

🎮 move group python Interface

📖 libfranka

🍿 franka_example_controllers

📺 rviz visualization

🖱️ rviz control

🗜️ gripper

🦿 joints

🎭 gazebo

📏 read cartesian coordinates

🚟 CoMPaPy control

🏁 usage

🥢 gripper

🌲 scene

🕹️ example

🐛 troubleshooting

🤔 known issues

👍 acknowledgements

About

Topics

Resources

License

Stars

Watchers

Forks

Releases

Packages 0

Languages

`CoMPaPy`

🔍 `libfranka`

🐼 `franka_ros`

🤸 `moveit_panda_config`

🤸 `compapy`

🎭 `venv`

✏️ `source_catkin` alias in `~/.bashrc`

🎮 `move group` python Interface

📖 `libfranka`

🍿 `franka_example_controllers`

📺 `rviz` visualization

🖱️ `rviz` control

🚟 `CoMPaPy` control

Packages