diff --git a/00_GettingStarted/docker/i2r/Dockerfile b/00_GettingStarted/docker/i2r/Dockerfile index 98c2b28317d4276b075787ace733abbe88879fca..78e359bc4f80514ea74e4d34c337fa7507a53888 100644 --- a/00_GettingStarted/docker/i2r/Dockerfile +++ b/00_GettingStarted/docker/i2r/Dockerfile @@ -11,3 +11,13 @@ RUN apt-get update && apt-get install -y \ python3-catkin-tools \ && rm -rf /var/lib/apt/lists/* +RUN rosdep update +RUN apt-get update && apt-get dist-upgrade -y \ +&& apt install -y ros-melodic-moveit \ +&& rm -rf /var/lib/apt/lists/* + +RUN cd ~/catkin_ws/src + +RUN cd ~/catkin_ws \ +&& catkin config --extend /opt/ros/${ROS_DISTRO} --cmake-args -DCMAKE_BUILD_TYPE=Release \ + diff --git a/00_GettingStarted/docker/i2r/catkin_ws/src/image.png b/00_GettingStarted/docker/i2r/catkin_ws/src/image.png new file mode 100644 index 0000000000000000000000000000000000000000..f7fb6d2bcddc62cd2dc205b748ace813060b5c60 Binary files /dev/null and b/00_GettingStarted/docker/i2r/catkin_ws/src/image.png differ diff --git a/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/CMakeLists.txt b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..4abca43ff4fa75b42d69edbbe8885ea766a5fc62 --- /dev/null +++ b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/CMakeLists.txt @@ -0,0 +1,211 @@ +cmake_minimum_required(VERSION 3.0.2) +project(task_2) + +## 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 + geometry_msgs + moveit_commander + moveit_msgs + 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# 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 test_sripts_v2 + CATKIN_DEPENDS geometry_msgs geometry_msgs moveit_commander moveit_msgs rospy tf trajectory_msgs tf2_ros +# 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}/test_sripts_v2.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/test_sripts_v2_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/move_arm.py scripts/pick_excercise_v1.py scripts/place_excercise_v1.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_test_sripts_v2.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) diff --git a/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/package.xml b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..dec39cecdb84c1df08efbaff1351f71153657519 --- /dev/null +++ b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/package.xml @@ -0,0 +1,80 @@ +<?xml version="1.0"?> +<package format="2"> + <name>task_2</name> + <version>0.0.0</version> + <description>The task_2 package</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="robotics@todo.todo">robotics</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/test_sripts_v2</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_msgs</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_msgs</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_msgs</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> diff --git a/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/move_arm.py b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/move_arm.py new file mode 100755 index 0000000000000000000000000000000000000000..b2ba64d8333b31d59591ae140d3df0d6cea2d4dd --- /dev/null +++ b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/move_arm.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python + +#### Task 2 + +#### Imports +import sys +import rospy +import math +from moveit_commander import RobotCommander, MoveGroupCommander +from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown +from geometry_msgs.msg import PoseStamped, Pose +import moveit_msgs.msg +from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation +from trajectory_msgs.msg import JointTrajectoryPoint +import geometry_msgs.msg +from tf import transformations +from copy import deepcopy + +#### Initialise a robotics node +roscpp_initialize(sys.argv) +rospy.init_node('picknplace_node', anonymous=True) +print('Pick n Place Node initialised') + +#### MoveIt APIs initialising +scene = PlanningSceneInterface() +robot = RobotCommander() +group_name="panda_arm" +move_group = MoveGroupCommander(group_name) + +#### Details +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:',planning_frame) +print('Robot current state:',robot.get_current_state()) + +#### Publisher for publishing the trajectories +display_trajectory_publisher = rospy.Publisher("/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20) + +#### Adding objects into the scene +## Q: Add the following set of objects with as per the specifications given + +table_id = 'table' +target_id = 'target' + +table_ground = 0.65 +table_size = [0.2, 0.7, 0.1] +target_size = [0.2, 0.1, 0.2] + +table_pose = PoseStamped() +table_pose.header.frame_id = planning_frame +table_pose.pose.position.x = 0.55 +table_pose.pose.position.y = 0.0 +table_pose.pose.position.z = table_ground + table_size[2] / 2.0 +table_pose.pose.orientation.w = 1.0 +scene.add_box(table_id, table_pose, table_size) + +target_pose = PoseStamped() +target_pose.header.frame_id = planning_frame +target_pose.pose.position.x = 0.55 +target_pose.pose.position.y = -0.1 +target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 +target_pose.pose.orientation.w = 1.0 +scene.add_box(target_id, target_pose, target_size) + +#### Specifying and executing a goal in joint states +tau = 2*math.pi +joint_goal = move_group.get_current_joint_values() +joint_goal[0] = 0 +joint_goal[1] = -tau / 8 +joint_goal[2] = 0 +joint_goal[3] = -tau / 4 +joint_goal[4] = 0 +joint_goal[5] = tau / 6 # 1/6 of a turn +joint_goal[6] = 0 + +move_group.go(joint_goal, wait=True) +move_group.stop() + +#### Move the robotics arm towards a pre-defined end effector pose +## Q: Move the end-effector to the predefined locaiton with value (x,y,z)=[.4,.1,.4] & w=[1] +## Hint: A) Set pose_goal using an instace of geometry_msgs.msg.Pose() B) Use function move_group.set_pose_target(); C)Execute the pose goal using function move_group.go() +pose_goal = geometry_msgs.msg.Pose() +pose_goal.orientation.w = 1.0 +pose_goal.position.x = 0.4 +pose_goal.position.y = 0.1 +pose_goal.position.z = 0.4 + +move_group.set_pose_target(pose_goal) +move_group.go(wait=True) + +#### For stopping and clearing pose targets +move_group.stop() +move_group.clear_pose_targets() + + + + +#### Set the support surface name to the table object +move_group.set_support_surface_name(table_id) + + + + + + + + + diff --git a/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/pick_excercise_v1.py b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/pick_excercise_v1.py new file mode 100755 index 0000000000000000000000000000000000000000..d0c3c24e3bde4d9870ed943eb6e60470652d4a26 --- /dev/null +++ b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/pick_excercise_v1.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python + +#### Task 2 + +#### Imports +import sys +import rospy +import math +from moveit_commander import RobotCommander, MoveGroupCommander +from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown +from geometry_msgs.msg import PoseStamped, Pose +import moveit_msgs.msg +from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation +from trajectory_msgs.msg import JointTrajectoryPoint +import geometry_msgs.msg +from tf import transformations +from copy import deepcopy +import tf2_ros +import tf +import math + +#### Initialise a robotics node +roscpp_initialize(sys.argv) +rospy.init_node('picknplace_node', anonymous=True) +print('Pick n Place Node initialised') + +#### MoveIt APIs initialising +scene = PlanningSceneInterface() +robot = RobotCommander() +group_name="panda_arm" +move_group = MoveGroupCommander(group_name) +move_group_gripper = MoveGroupCommander("panda_hand") + +#### Open and close gripper +move_group_gripper.set_named_target("close") +move_group_gripper.go() +move_group_gripper.set_named_target("open") +move_group_gripper.go() +rospy.sleep(5) + + +#### Details +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:',planning_frame) +print('Robot current state:',robot.get_current_state()) +rospy.sleep(2) + +#### Publisher for publishing the trajectories +display_trajectory_publisher = rospy.Publisher("/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20) +rospy.sleep(2) +print("Publisher for publishing trajectories created") +#### Remove any pending objects +scene.remove_world_object("table1") +scene.remove_world_object("table2") +scene.remove_world_object("object1") +rospy.sleep(5) + +print("Removed any pending objects") + + +table_id = 'table' +box1_id = 'box1' +box2_id = 'box2' +target_id = 'target' +tool_id = 'tool' + +table_ground = 0.65 +table_size = [0.2, 0.7, 0.01] +box1_size = [0.1, 0.05, 0.05] +box2_size = [0.05, 0.05, 0.15] +target_size = [0.02, 0.01, 0.12] + + +#### Placing objects +def create_objects(): + ''' + table1 = PoseStamped() + table1.header.frame_id = planning_frame + table1.pose.position.x = 0.5 + table1.pose.position.y = 0.0 + table1.pose.position.z = 0.2 + scene.add_box("table1", table1, (0.2, 0.4, 0.4)) + + table2 = PoseStamped() + table2.header.frame_id = planning_frame + table2.pose.position.x = 0.0 + table2.pose.position.y = 0.5 + table2.pose.position.z = 0.2 + scene.add_box("table2", table2, (0.4, 0.2, 0.4)) + ''' + + table_pose = PoseStamped() + table_pose.header.frame_id = planning_frame + table_pose.pose.position.x = 0.55 + table_pose.pose.position.y = 0.1 + table_pose.pose.position.z = ( table_ground + table_size[2] / 2.0) -0.27 + table_pose.pose.orientation.w = 1.0 + scene.add_box(table_id, table_pose, table_size) + + box1_pose = PoseStamped() + box1_pose.header.frame_id = planning_frame + box1_pose.pose.position.x = 0.55 + box1_pose.pose.position.y = -0.13 + box1_pose.pose.position.z =( table_ground + table_size[2] + box1_size[2] / 2.0) -0.27 + box1_pose.pose.orientation.w = 1.0 + scene.add_box(box1_id, box1_pose, box1_size) + + box2_pose = PoseStamped() + box2_pose.header.frame_id = planning_frame + box2_pose.pose.position.x = 0.54 + box2_pose.pose.position.y = 0.33 + box2_pose.pose.position.z = (table_ground + table_size[2] + box2_size[2] / 2.0) -0.27 + box2_pose.pose.orientation.w = 1.0 + scene.add_box(box2_id, box2_pose, box2_size) + + object1 = PoseStamped() + object1.header.frame_id = planning_frame + object1.pose.position.x = 0.5 + object1.pose.position.y = 0.0 + object1.pose.position.z = 0.5 + scene.add_box("object1", object1, (0.02, 0.02, 0.2)) + + move_group.set_support_surface_name("table1") + + +create_objects() +rospy.sleep(5) + +print("Created objects") + +#### Defining a grasp +grasps=[] +g = Grasp() +g.id = "test" +grasp_pose = PoseStamped() +grasp_pose.header.frame_id = planning_frame +grasp_pose.pose.position.x = 0.55 +grasp_pose.pose.position.y = 0.0 +grasp_pose.pose.position.z = 0.5 +tau=math.pi*2 +quaternion = tf.transformations.quaternion_from_euler(-tau / 4, -tau / 8, -tau / 4) +grasp_pose.pose.orientation.x=quaternion[0] +grasp_pose.pose.orientation.y=quaternion[1] +grasp_pose.pose.orientation.z=quaternion[2] +grasp_pose.pose.orientation.w=quaternion[3] +g.grasp_pose = grasp_pose + +#### Checking grasp pose +#move_group.set_pose_target(grasp_pose) +#move_group.go() + +#### Pre-grasp approach +g.pre_grasp_approach.direction.header.frame_id = planning_frame +g.pre_grasp_approach.direction.vector.x = 1.0 +g.pre_grasp_approach.min_distance = 0.095 +g.pre_grasp_approach.desired_distance = 0.115 + + +#### set the pre-grasp posture +g.pre_grasp_posture.header.frame_id = planning_frame +g.pre_grasp_posture.joint_names = ["panda_finger_joint1","panda_finger_joint2"] + +pos_open = JointTrajectoryPoint() +pos_open.positions.append(float(0.04)) +g.pre_grasp_posture.points.append(pos_open) +#g.pre_grasp_posture.points.append(pos_open) + +#### set the post-grasp retreat +g.post_grasp_retreat.direction.header.frame_id = planning_frame +g.post_grasp_retreat.direction.vector.z = 1.0 +g.post_grasp_retreat.min_distance = 0.1 +g.post_grasp_retreat.desired_distance = 0.25 + +#### set the grasp posture +g.grasp_posture.header.frame_id = planning_frame +g.grasp_posture.joint_names = ["panda_finger_joint1","panda_finger_joint2"] + +pos_close = JointTrajectoryPoint() +pos_close.positions.append(float(0.00)) +g.grasp_posture.points.append(pos_close) +#g.grasp_posture.points.append(pos_close) + +#### Other grasp setings +g.allowed_touch_objects = ["object1"] + +#### Grasping +grasps.append(g) +move_group.pick("object1", grasps) + + + + + diff --git a/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/place_excercise_v1.py b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/place_excercise_v1.py new file mode 100755 index 0000000000000000000000000000000000000000..6e8065b7d1a64038551fef476975f4e29b40bda0 --- /dev/null +++ b/00_GettingStarted/docker/i2r/catkin_ws/src/task_2/scripts/place_excercise_v1.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python + +#### Task 2 + +#### Imports +import sys +import rospy +import math +from moveit_commander import RobotCommander, MoveGroupCommander +from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown +from geometry_msgs.msg import PoseStamped, Pose +import moveit_msgs.msg +from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation +from trajectory_msgs.msg import JointTrajectoryPoint +import geometry_msgs.msg +from tf import transformations +from copy import deepcopy +#from tf2_ros import Quaternion +import tf2_ros +import tf +import math + +tau=2*math.pi + +#### Initialise a robotics node +roscpp_initialize(sys.argv) +rospy.init_node('picknplace_node', anonymous=True) +print('Pick n Place Node initialised') + +#### MoveIt APIs initialising +scene = PlanningSceneInterface() +robot = RobotCommander() +group_name="panda_arm" +move_group = MoveGroupCommander(group_name) + + +#### Details +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:',planning_frame) +print('Robot current state:',robot.get_current_state()) +rospy.sleep(2) + +#### Remove any pending objects +scene.remove_world_object("table1") +scene.remove_world_object("table2") +rospy.sleep(5) + +#### Adding new object +table2 = PoseStamped() +table2.header.frame_id = planning_frame +table2.pose.position.x = 0.0 +table2.pose.position.y = -0.5 +table2.pose.position.z = 0.2 +scene.add_box("table2", table2, (0.4, 0.2, 0.4)) + + +#### Details +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:',planning_frame) +print('Robot current state:',robot.get_current_state()) +rospy.sleep(2) + +#### Placing pose definition +place_pose=PoseStamped() +place_pose.header.frame_id = planning_frame +quaternion = tf.transformations.quaternion_from_euler(0, 0, -tau / 4) +place_pose.pose.position.x = 0 +place_pose.pose.position.y = -0.5 +place_pose.pose.position.z = 0.5 +place_pose.pose.orientation.x=quaternion[0] +place_pose.pose.orientation.y=quaternion[1] +place_pose.pose.orientation.z=quaternion[2] +place_pose.pose.orientation.w=quaternion[3] + +#### Checking place pose +#move_group.set_pose_target(place_pose) +#move_group.go() + +place_location=PlaceLocation() +place_location.place_pose.header.frame_id = planning_frame +place_location.place_pose=place_pose + + +#### Pre-place approach +place_location.pre_place_approach.direction.header.frame_id = planning_frame +place_location.pre_place_approach.direction.vector.z = -1.0 +place_location.pre_place_approach.min_distance = 0.095 +place_location.pre_place_approach.desired_distance = 0.115 + + +#### Placing +move_group.set_support_surface_name("table2") +move_group.place("object1", place_location) + + diff --git a/00_GettingStarted/docker/i2r/docker-compose.yml b/00_GettingStarted/docker/i2r/docker-compose.yml index e90b35a6537c2444c6d8c78d36d69a89b7fd01dc..e22dd9681768cac57a1dd7c6ffdc793c77a66c0f 100644 --- a/00_GettingStarted/docker/i2r/docker-compose.yml +++ b/00_GettingStarted/docker/i2r/docker-compose.yml @@ -16,6 +16,9 @@ services: ports: - "8080:8080" environment: + # Adjust to your screen size + - DISPLAY_WIDTH=1800 + - DISPLAY_HEIGHT=1050 - RUN_XTERM=no volumes: