Skip to content
Snippets Groups Projects
Commit e2340594 authored by holzheim's avatar holzheim
Browse files

Updated documentation

parent 3f53d6e6
Branches
No related tags found
No related merge requests found
Showing
with 510 additions and 11 deletions
moveit_tutorials @ f77a29c7
Subproject commit f77a29c7c3281ae82c6a4d793c39bed61c45cfb4
panda_moveit_config @ 625be994
Subproject commit 625be99465d1cf897297f0fde414f90349e3015f
...@@ -45,7 +45,7 @@ print('Robot groups available:',group_names) ...@@ -45,7 +45,7 @@ print('Robot groups available:',group_names)
planning_frame = move_group.get_planning_frame() planning_frame = move_group.get_planning_frame()
print('Reference frame:',planning_frame) print('Reference frame:',planning_frame)
eef_link = move_group.get_end_effector_link() eef_link = move_group.get_end_effector_link()
print('End effector link:',planning_frame) print('End effector link:',eef_link)
print('Robot current state:',robot.get_current_state()) print('Robot current state:',robot.get_current_state())
rospy.sleep(2) rospy.sleep(2)
......
cmake_minimum_required(VERSION 3.0.2)
project(testing_moveit)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
moveit_commander
moveit_core
moveit_msgs
roscpp
rospy
tf
tf2_ros
trajectory_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# moveit_msgs# trajectory_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
LIBRARIES testing_moveit
CATKIN_DEPENDS geometry_msgs moveit_commander moveit_core moveit_msgs roscpp rospy tf tf2_ros trajectory_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/testing_moveit.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/testing_moveit_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/testing_moveit.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_testing_moveit.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
<?xml version="1.0"?>
<package format="2">
<name>testing_moveit</name>
<version>0.0.0</version>
<description>The testing_moveit package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="root@todo.todo">root</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/testing_moveit</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>moveit_commander</build_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>moveit_commander</build_export_depend>
<build_export_depend>moveit_core</build_export_depend>
<build_export_depend>moveit_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>tf</build_export_depend>
<build_export_depend>tf2_ros</build_export_depend>
<build_export_depend>trajectory_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>moveit_commander</exec_depend>
<exec_depend>moveit_core</exec_depend>
<exec_depend>moveit_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
#!/usr/bin/env python
import sys
import copy
import math
import rospy
import moveit_commander
import geometry_msgs.msg
import moveit_msgs.msg
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
print("--- Imports done! ---")
#### Initialise a robotics node
print("--- Starting initialising a robotics node ---")
moveit_commander.roscpp_initialize(sys.argv)
print("Executed: moveit_commander.roscpp_initialize(sys.argv)")
rospy.init_node('testing_moveit_node', anonymous=True)
print("Executed: rospy.init_node('testing_moveit_node', anonymous=True)")
#### MoveIt APIs initialising
print("--- MoveIt APIs initialising ---")
scene = moveit_commander.PlanningSceneInterface()
print("Executed: scene = moveit_commander.PlanningSceneInterface()")
robot = moveit_commander.RobotCommander()
print("Executed: robot = moveit_commander.RobotCommander()")
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
print("Executed = move_group = moveit_commander.MoveGroupCommander(group_name)")
move_group_gripper = moveit_commander.MoveGroupCommander("panda_hand")
print("Executed: move_group_gripper = moveit_commander.MoveGroupCommander(\"panda_hand\")")
#### Open and close gripper
print("--- Open and close gripper ---")
move_group_gripper.set_named_target("close")
print("Executed: move_group_gripper.set_named_target(\"close\")")
rospy.sleep(5)
move_group_gripper.go()
print("Exectued: move_group_gripper.go()")
rospy.sleep(5)
move_group_gripper.set_named_target("open")
print("Executed: move_group_gripper.set_named_target(\"open\")")
rospy.sleep(5)
move_group_gripper.go()
print("Exectued: move_group_gripper.go()")
rospy.sleep(5)
print("Executed: rospy.sleep(5)")
rospy.sleep(5)
#### Details
print("--- Getting Information about the robot ---")
group_names = robot.get_group_names()
print('Robot groups available:',group_names)
planning_frame = move_group.get_planning_frame()
print('Reference frame:', planning_frame)
eef_link = move_group.get_end_effector_link()
print("End effector link:", eef_link)
rcs = robot.get_current_state()
print('Robot current state:', rcs)
rospy.sleep(5)
#### Publisher for publishing the trajectories
print("--- Creating Publisher to publish trajectories ---")
display_trajectories_publisher = rospy.Publisher("/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20)
print("Executed: display_trajectories_publisher = rospy.Publisher(\"/move_group/display_planned_path\", moveit_msgs.msg.DisplayTrajectory, queue_size=20)")
rospy.sleep(5)
print('Planning scene objects:', scene.get_objects())
scene.remove_world_object("table1")
scene.remove_world_object("table2")
scene.remove_world_object("object1")
rospy.sleep(5)
#### Defining Objects
# object IDs
table_id = 'table'
box1_id = 'box1'
target_id = 'target'
tool_id = 'tool'
table_ground = 0.65
table_size = [0.2, 0.7, 0.01]
box_1_size = [0.1, 0.05, 0.05]
target_size = [0.02, 0.01, 0.12]
##### Placing objects
table_pose = geometry_msgs.msg.PoseStamped()
table_pose.header.frame_id = planning_frame
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0
table_pose.pose.position.z = (table_ground + table_size[2]/2)
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
rospy.sleep(5)
\ No newline at end of file
...@@ -10,13 +10,14 @@ This exercise is about to work with robotic arms in simulation and visualization ...@@ -10,13 +10,14 @@ This exercise is about to work with robotic arms in simulation and visualization
> **Learning objective**: > **Learning objective**:
> >
> After completion of exercise 3 you should know... > After completion of exercise 3 you should know...
>
>- ... what PID control is.
>- ... which effect the PID constants have on a robot.
>- ... how to perform a pick and place task using the MoveIt framework.
- ... what PID control is. ## Task 3.1: Controller Tuning
- ... which effect the PID constants have on a robot.
- ... how to perform a pick and place task using the MoveIt framework.
## Controller Tuning
### <u>Task setup</u>
To visualize which effect a properly tuned PID controller has and how the behavior of the robot is affected by the different terms (**P**roportinal, **I**ntegral, **D**erivative) we will facilitate the `UR5` robot inside of a simulation envrionment in `ROS`. The objective will be to tune the PID constants ($K_P$, $K_I$, $K_D$) of the `UR5` wrist so that overshoot and settling time are reduced. To document and visualize the progress of the tuning process, you can use the ROS GUI framework `rqt` to plot the reference and measured joint state. To visualize which effect a properly tuned PID controller has and how the behavior of the robot is affected by the different terms (**P**roportinal, **I**ntegral, **D**erivative) we will facilitate the `UR5` robot inside of a simulation envrionment in `ROS`. The objective will be to tune the PID constants ($K_P$, $K_I$, $K_D$) of the `UR5` wrist so that overshoot and settling time are reduced. To document and visualize the progress of the tuning process, you can use the ROS GUI framework `rqt` to plot the reference and measured joint state.
### Installation of necessary packages ### Installation of necessary packages
...@@ -66,7 +67,7 @@ Befor we can start with the tuning process of the PID controller, some packages ...@@ -66,7 +67,7 @@ Befor we can start with the tuning process of the PID controller, some packages
Now all packages are installed that are necessary to work with the `UR5` and tune the PID controller constants. Now all packages are installed that are necessary to work with the `UR5` and tune the PID controller constants.
## Setting up the working environment ### Setting up the working environment
Following, the instructions to set up and environment to inspect and document the influence of the PID controller constants $K_P$, $K_I$ and $K_D$ on the performance of the given `UR5` robotic arm manipulator. The [`Gazebo`](https://gazebosim.org/home) simulation envrionment and the `rqt` (gui) framework with it's build in plugins to publish messages to topics, visulaize messages and manipulate (controller) parameters will be used. Following, the instructions to set up and environment to inspect and document the influence of the PID controller constants $K_P$, $K_I$ and $K_D$ on the performance of the given `UR5` robotic arm manipulator. The [`Gazebo`](https://gazebosim.org/home) simulation envrionment and the `rqt` (gui) framework with it's build in plugins to publish messages to topics, visulaize messages and manipulate (controller) parameters will be used.
...@@ -101,15 +102,116 @@ Now the `rqt` environment will be set up to interact and visualize the interacti ...@@ -101,15 +102,116 @@ Now the `rqt` environment will be set up to interact and visualize the interacti
```bash ```bash
rosrun rqt_gui rqt_gui rosrun rqt_gui rqt_gui
``` ```
- In the opening window select the *Plugins* tab, go to the *Topics* option and select *Message Publisher*. The *Message Publisher* plugin allows to publish to a certain topic using a GUI. Add the ***/wrist_1_joint_position_controller/command*** by selecting it in the drop down menu and click the green plus sign. In the widget below, the message that should be published to the topic can be set by extending the topic and setting the expression column to the desired value. Start by setting the expression to a static value. To move the robots wrist joint the message expects the rotation to be given in radiant. So set the expression to -1.57 (= $\frac{\pi}{2}$) which is equal to a 90° rotation. As long as the checkbox in the left is checked for a certain topic, the message gets published to the topic. - In the opening window select the *Plugins* tab, go to the *Topics* option and select *Message Publisher*. The *Message Publisher* plugin allows to publish to a certain topic using a GUI. Add the ***/wrist_1_joint_position_controller/command*** by selecting it in the drop down menu and click the green plus sign. Once you add the topic it should look something like shown in Fig 1. In the widget below, the message that should be published to the topic can be set by extending the topic and setting the expression column to the desired value. Start by setting the expression to a static value. To move the robots wrist joint the message expects the rotation to be given in radiant. So set the expression to -1.57 (= $\frac{\pi}{2}$) which is equal to a 90° rotation. As long as the checkbox in the left is checked for a certain topic, the message gets published to the topic.
| ![topic_pub](images/topic_pub.png) |
|:--:|
| <b>Fig 1</b>|
- Set different values for the wrist joint and see how the joint state gets adjusted in `Gazebo`. - Set different values for the wrist joint and see how the joint state gets adjusted in `Gazebo`.
- To visualize the influence of the PID constants, use the rqt plot plugin to plot the reference value and the measured output of the wrist joint position. Select the *Plugins* option, go to the *Visualization* option and select the *Plot* plugin. - To visualize the influence of the PID constants, use the rqt plot plugin to plot the reference value and the measured output of the wrist joint position. Select the *Plugins* option, go to the *Visualization* option and select the *Plot* plugin.
Add the ***/wrist_1_joint_position_controller/state/process_value*** topic to which the measured joint position is published to and the ***/wrist_1_joint_position_controller/command/data*** topic to which the reference value for the joint is published to. The plot is now joint position over time. Add the ***/wrist_1_joint_position_controller/state/process_value*** topic to which the measured joint position is published to and the ***/wrist_1_joint_position_controller/command/data*** topic to which the reference value for the joint is published to. The plot is now joint position over time. Once you add both these topics you will see a graph and this will show you how the current position of the wrist changes with respect to the set value.
| ![rqt_plot](images/rqt_plot.png) |
|:--:|
| <b>Fig 2</b>|
- Now make the wrist joint change it's angle by using the Message Publisher we set up before and see how the measured joint position approaches the reference value. Like shown in the lecture, depending on the PID constants set for that specific joint, the joint approaches the reference state either underdamped, overdamped or critically damped. - Now make the wrist joint change it's angle by using the Message Publisher (Fig 1) we set up before and see how the measured joint position approaches the reference value. Like shown in the lecture, depending on the PID constants set for that specific joint, the joint approaches the reference state either underdamped, overdamped or critically damped.
- To tune the PID constants, select the *Plugins* tab, go to the *Configuration* option and select the *Dynamic Reconfigure* plugin. Using that plugin it's possible to adjust the PID constants for each specific joint. Open the pid constants for the ***/wrist_1_joint_position_controller***. - To tune the PID constants, select the *Plugins* tab, go to the *Configuration* option and select the *Dynamic Reconfigure* plugin. Using that plugin it's possible to adjust the PID constants for each specific joint. Open the pid constants for the ***/wrist_1_joint_position_controller*** (indicated in Fig 3).
| ![dynamic_reconfigure](images/dyn_recon.png) |
|:--:|
| <b>Fig 3</b>|
- With that set up, you can now tune your PID constants, change the joint angle using the Message Publisher and plot the reference joint position and the measured joint position. Use that environment to optimize the PID constants to reach the desired behavior. - With that set up, you can now tune your PID constants, change the joint angle using the Message Publisher and plot the reference joint position and the measured joint position. Use that environment to optimize the PID constants to reach the desired behavior.
### <u>Your Task</u>
1. Your task is to tune the PID controller for `wrist_1_joint_position_controller` to make the writs move smoothly to the position that you set using the `/wrist_1_joint_position_controller/command` topic that we set up in **Fig 1**.
2. As you start tuning the PID parameter you will see the graph (shown in Fig 2) will change showing the response of writs to different wrist values. Your initial graph will look like **Fig 4** (before tuning the PID parameters).
| ![untuned_wrist](images/result.png) |
|:--:|
| <b>Fig 4</b>|
3. **Fig 4** shows how an untuned wrist joint will react to any given position. Your task is to tune the `PID` parameters using the Dynamic Reconfigure(shown in Fig 3) to make the writs move smooth to the set position shown in **Fig 5**
| ![untuned_wrist](images/un_tuned.gif) |
|:--:|
| <b>Fig 5</b>|
| ![tuned_wrist](images/tuned.gif) |
|:--:|
| <b>Fig 6</b>|
## Pick and Place
In the previous section you learnt which effect a controller and its parameters have on an robotic arm. As a next step, we use the robotic arm *Panda* from [Franka Emika](https://www.franka.de/) which comes already with a tuned controller. We will control the simulation of the robot inside the `ROS` environment using the [MoveIt Motion Planning Framework](https://moveit.ros.org/) to perform a simple pick and place task.
- Change into the *src* folder of your `catkin workspace`.
```bash
cd ~/catkin_ws/src
```
- Clone the following packages from git.
```bash
git clone https://github.com/ros-planning/moveit_tutorials.git -b melodic-devel
```
```bash
git clone https://github.com/ros-planning/panda_moveit_config.git -b melodic-devel
```
- Install now all package dependencies that are missing in your workspace but needed in the just installed packages.
```bash
rosdep update
```
```bash
apt update
```
```bash
rosdep install -y --from-paths . --ignore-src --rosdistro melodic
```
- Change into your `catkin workspace` and **build** the packages.
```bash
cd ~/catkin_ws
```
```bash
catkin build --jobs 1
```
- Source the `catkin workspace`.
```bash
source ~/.bashrc
```
- Now launch the panda robot in `RViz`.
```bash
roslaunch panda_moveit_config demo.launch
```
### <u>Your Task</u>
After using the GUI to move the manipulator to a goal position and perform the path planning for that pose your task is now to get familiar with the `Python` interface of the `MoveIt` framework. You can find already two implemented `Python` scripts inside the *catkin_ws/src/task_2/scripts* folder. Open them in `Visual Studio Code` to get familiar with the functions used to get an impression what should happen to the `Panda` robot if the code get executed.
3. **Debug** the source code of the scripts *pick_exercise_v1.py* and *place_exercise_v1.py*. The scripts should finally perform the following tasks:
- When executing *pick_exercise_v1.py* the `Panda`robot should **pick** one of the created objects.
- Afterwards, when executing the *place_exercise_v1.py* script, the object should be moved to a goal/new position and droped there.
- After you successful debugging, create a new table/plate inside of the *pick_exercise_v1.py* script and modify the script in such a way that it moves all objects one after another from the old table/plate to the newly created one.
You can find further information about using the `MoveIt` framework [here](http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html).
\ No newline at end of file
03_ArmManipulator/images/dyn_recon.png

47.7 KiB

03_ArmManipulator/images/result.png

25.7 KiB

03_ArmManipulator/images/rqt_plot.png

38.5 KiB

03_ArmManipulator/images/topic_pub.png

40.3 KiB

03_ArmManipulator/images/tuned.gif

698 KiB

03_ArmManipulator/images/un_tuned.gif

2.14 MiB

0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment