-
Co
ntrol withM
oveIt ofPa
nda robot inPy
thon -
๐ฃ๏ธ
CoMPaPy
=[kษm papi]
๐ด (๐ซ๐ท) -
๐ง work in progress ๐ท
"a simple python interface for basic control of a real panda robot with moveit"
- using only official and maintained repos:
franka_ros
andMoveIt
- two modes:
simulated
andreal
panda - no
cpp
, justpython
- 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 inmove_l
- e.g.
- control
panda_link8
instead of the gripper ๐ฉ- c.f. known issues
- todo: ros.org/question/334902
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
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
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 |
follow these instructions
follow these instructions
- prefer
catkin build
tocatkin_make
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
follow these instructions
- prefer
~/catkin_ws/src
to~/ws_moveit/src
cd ~/catkin_ws/src
git clone https://github.com/chauvinSimon/compapy.git
cd ~/catkin_ws/
catkin build
cd ~/catkin_ws/src
python3 -m venv ven --system-site-packages
source ven/bin/activate
pip install -r requirements.txt
this may be useful
alias source_catkin="source ~/catkin_ws/devel/setup.bash; source ~/catkin_ws/src/ven/bin/activate; cd ~/catkin_ws/src"
"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
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}
againstModuleNotFoundError
andModuleNotFoundError
the first time: follow the fci instructions to configure fci
switch on the robot
- release joints
- activate
fci
- Re-initialize Hand
in a terminal
source_catkin
(the alias defined in a previous section)
-
move the arm
- in
programming
mode - see this 30-sec demo
- in
-
open/close the gripper
- in
programming
mode orexecution
mode - see this 15-sec demo
FCI
can stay activated but make sure to closeRViz
and stop allROS
nodes (otherwiseEnd Effector: Not Connected
)
- in
ping 172.16.0.2
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
in execution
mode
~/libfranka/build/examples/communication_test 172.16.0.2
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, inrad/s
, is used to control the duration of the move, hence the speed
- the
-
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'?
-
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'?
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
, enableQuery Goal State
- drag the interactive marker to some the goal state
- click
Plan
and theExecute
- try different planning settings (cartesian path, speed, planning pipeline ...)
- try to display and hide the different visualisation tools
- I personally like the
Joints
tab inMotionPlanning
plugin
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 }"
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
๐ฝ๏ธ ground_plane
about ground_plane
- the default
world
contains aground_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
inModels
- or ... create a
my_empty.world
with just asun
, and add the argsworld:=[ABSOLUTE_PATH_TO_my_empty.world]
to thedemo_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
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
addtf
underPanels
- 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
- either manually in
- read the updated transform
alternatively, without rviz
:
rosrun tf tf_echo /panda_link0 /panda_EE
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
adjust the parameters for close_gripper()
, depending on the width of the object to grasp, in compapy.yaml
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
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
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]
๐ฏ [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
andjump_threshold
params ofcompute_cartesian_path()
, e.g. several trials with random offsets- I observe
fraction
improvements of max.1%
, i.e. so not very helpful
- I observe
- add intermediate waypoints to
waypoints
to make sub-paths easier - change the
orientation
of thetarget_pose
- I noticed that simple combinations of [straight lines] and [rotation around
z
of the gripper ofrz
deg] fail for certainrz
values
- I noticed that simple combinations of [straight lines] and [rotation around
- 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 thepipeline
argument) - play with the
jump_threshold
param ofcompute_cartesian_path()
- reduce the degrees of freedom (dof) of the panda arm
- ideally, create a new
move_group
using theMoveIt setup assistant
where not all joints are used- e.g. freeze
joint_3
andjoint_5
- todo: I could not find how to do it
- e.g. freeze
- alternatively, but not optimal, drastically reduce the bounds of
joint_3
andjoint_5
injoint_limits.yaml
(note thatfr3
is more constrained thanpanda
)moveit
uses the limits defined in~/catkin_ws/src/franka_ros/franka_description/robots/panda/joint_limits.yaml
โ ๏ธ after adjustingjoint_limits.yaml
, make sure all joints are in their intervals before startingmoveit
in theexecution
mode! Otherwise, the arm can strongly vibrate- in
execution
mode, runpython scripts/move_to_start_and_set_limits.py --dof=5
, which does the following:- clear changes in
joint_limits.yaml
, i.e. reset to7
-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, withroslaunch compapy real.launch robot_ip:=172.16.0.2
, manually move the amr so thatjoint_3
andjoint_5
are at0
(align the arrows printed on the joints) - make sure these two joints are at
0
withrosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller
- manually move the other joints roughly to the middle of their ranges
- quit
rviz
and killreal.launch
- in
- note: this can also be achieved using
- overwrite
joint_limits.yaml
for5
-dof: reduce the bounds ofjoint_3
andjoint_5
to ~0
- clear changes in
- ideally, create a new
๐ฒ [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
andnumpy
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 betweenpanda_EE
andpanda_link8
is~103
mm - the origin of the
panda_EE
frame is located between the finger pads, at~8
mm from the tip
![]() |
---|
origin of panda_EE , at ~8.5 mm from the tip |
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)