{
"cells": [
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true,
"pycharm": {
"name": "#%%\n"
}
},
"outputs": [],
"source": [
"%%bash --bg\n",
"rviz -d ~/giskard_examples/launch/rviz_config/standalone.rviz"
]
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"# imports and helper functions\n",
"import rospy\n",
"from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3Stamped, PointStamped, Vector3\n",
"from tf.transformations import quaternion_from_matrix, quaternion_about_axis\n",
"from copy import deepcopy\n",
"from giskard_msgs.msg import WorldBody, CollisionEntry, WorldGoal, GiskardError\n",
"import numpy as np\n",
"from giskardpy.goals.joint_goals import JointPositionList\n",
"from IPython.display import display, Image\n",
"from pdf2image import convert_from_path\n",
"import os\n",
"import glob\n",
"import ipywidgets as widgets\n",
"from IPython.display import display\n",
"import subprocess\n",
"\n",
"\n",
"# Define some helper functions\n",
"def reset_giskard():\n",
" giskard.clear_motion_goals_and_monitors()\n",
" giskard.world.clear()\n",
" if ROBOT == 'pr2':\n",
" default_pose = {\n",
" 'r_elbow_flex_joint': -0.15,\n",
" 'r_forearm_roll_joint': 0,\n",
" 'r_shoulder_lift_joint': 0,\n",
" 'r_shoulder_pan_joint': 0,\n",
" 'r_upper_arm_roll_joint': 0,\n",
" 'r_wrist_flex_joint': -0.10001,\n",
" 'r_wrist_roll_joint': 0,\n",
" 'l_elbow_flex_joint': -0.15,\n",
" 'l_forearm_roll_joint': 0,\n",
" 'l_shoulder_lift_joint': 0,\n",
" 'l_shoulder_pan_joint': 0,\n",
" 'l_upper_arm_roll_joint': 0,\n",
" 'l_wrist_flex_joint': -0.10001,\n",
" 'l_wrist_roll_joint': 0,\n",
" 'torso_lift_joint': 0.2,\n",
" 'head_pan_joint': 0,\n",
" 'head_tilt_joint': 0,\n",
" 'l_gripper_l_finger_joint': 0.55,\n",
" 'r_gripper_l_finger_joint': 0.55\n",
" }\n",
" done = giskard.monitors.add_set_seed_configuration(default_pose)\n",
" base_pose = PoseStamped()\n",
" base_pose.header.frame_id = 'map'\n",
" base_pose.pose.position = Point(0, 0, 0)\n",
" base_pose.pose.orientation.w = 1\n",
" done2 = giskard.monitors.add_set_seed_odometry(base_pose=base_pose)\n",
" giskard.motion_goals.allow_all_collisions()\n",
" giskard.monitors.add_end_motion(start_condition=f'{done} and {done2}')\n",
" giskard.execute()\n",
" giskard.clear_motion_goals_and_monitors()\n",
"\n",
"\n",
"# global variables\n",
"ROBOT = 'pr2'\n",
"single_joint_goal = {'torso_lift_joint': 0.3}\n",
"tool_frame = 'l_gripper_tool_frame'\n",
"cam_frame = 'wide_stereo_optical_frame'\n",
"base_link = 'base_footprint'\n",
"gripper_joint = 'l_gripper_l_finger_joint'\n",
"gripper_joint_open = 0.55\n",
"gripper_joint_close = 0.1\n",
"\n",
"# List of available launch files\n",
"launch_files = {\n",
" 'PR2': 'giskardpy_pr2_standalone_vrb.launch'\n",
"}\n",
"\n",
"# Dropdown widget\n",
"dropdown = widgets.Dropdown(\n",
" options=launch_files.keys(),\n",
" value='PR2',\n",
" description='Select Robot:',\n",
" style={'description_width': 'initial'},\n",
" layout=widgets.Layout(width='200px')\n",
")\n",
"\n",
"# Button widget\n",
"button = widgets.Button(\n",
" description='Start Launch File',\n",
" button_style='success',\n",
")\n",
"\n",
"\n",
"def update_global_variables(robot):\n",
" global ROBOT\n",
" global single_joint_goal\n",
" global tool_frame\n",
" global cam_frame\n",
" global base_link\n",
" global gripper_joint\n",
" global gripper_joint_open\n",
" global gripper_joint_close\n",
" ROBOT = robot\n",
" if ROBOT == 'stretch':\n",
" single_joint_goal = {'joint_lift': 0.7}\n",
" tool_frame = 'link_grasp_center'\n",
" cam_frame = 'camera_color_optical_frame'\n",
" base_link = 'base_link'\n",
" gripper_joint = 'joint_gripper_finger_left'\n",
" gripper_joint_open = 0.55\n",
" gripper_joint_close = 0.1\n",
" elif ROBOT == 'pr2':\n",
" single_joint_goal = {'torso_lift_joint': 0.3}\n",
" tool_frame = 'l_gripper_tool_frame'\n",
" cam_frame = 'wide_stereo_optical_frame'\n",
" base_link = 'base_footprint'\n",
" gripper_joint = 'l_gripper_l_finger_joint'\n",
" gripper_joint_open = 0.55\n",
" gripper_joint_close = 0.1\n",
"\n",
"\n",
"# Function to start the selected ROS launch file\n",
"def on_button_click(b):\n",
" selected_launch_file = launch_files[dropdown.value]\n",
"\n",
" update_global_variables(dropdown.value.lower())\n",
"\n",
" try:\n",
" result = subprocess.run('rosnode kill /giskard', shell=True, check=True, stdout=subprocess.PIPE,\n",
" stderr=subprocess.PIPE)\n",
" print(result.stdout.decode())\n",
" print(result.stderr.decode())\n",
" except subprocess.CalledProcessError as e:\n",
" print(f\"Error occurred: {e.stderr.decode()}\")\n",
"\n",
" print(f\"Starting ROS launch file: {selected_launch_file}\")\n",
" command = f\"roslaunch giskardpy {selected_launch_file}\"\n",
" LAUNCH_PROCESS = subprocess.Popen(['/bin/bash', '-c', command],\n",
" stdout=subprocess.DEVNULL,\n",
" stderr=subprocess.DEVNULL, shell=False)\n",
"\n",
"\n",
"# Attach the event handler to the button\n",
"button.on_click(on_button_click)\n",
"\n",
"# Display widgets\n",
"display(dropdown)\n",
"display(button)\n",
"rospy.init_node('giskard_examples')"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"# Hints\n",
"Poses are fully described by a position `PoseStamped().pose.position.x/.y/.z` and an orientation given as a quaternion `PoseStamped().pose.orientation.x/.y/.z/.w`.\n",
"An identity quaternion has the values (0, 0, 0, 1) for (x, y, z, w).\n",
"Here quaternions can be calculated by different functions:\n",
"- quaternion_from_matrix(4x4 array for rotation matrix)\n",
"- quaternion_about_axis(angle, axis=[x, y, z])\n"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"from giskardpy.python_interface.python_interface import GiskardWrapper\n",
"\n",
"giskard = GiskardWrapper()"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"Execute the following cell to reset the state of the robot and the world.\n",
"Also needed to initially load the world."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"reset_giskard()\n",
"\n",
"kitchen_pose = PoseStamped()\n",
"kitchen_pose.header.frame_id = 'map'\n",
"kitchen_pose.pose.orientation.w = 1\n",
"giskard.world.add_urdf(name='iai_kitchen',\n",
" urdf=rospy.get_param('kitchen_description'),\n",
" pose=kitchen_pose)\n",
"box_name = 'box'\n",
"fridge_handle_name = 'iai_fridge_door_handle'\n",
"fridge_shelf_name = 'iai_fridge_door_shelf1_bottom'\n",
"sink_area = 'sink_area_surface'\n",
"left_gripper = 'l_gripper_tool_frame'\n",
"right_gripper = 'r_gripper_tool_frame'\n",
"\n",
"box_pose = PoseStamped()\n",
"box_pose.header.frame_id = sink_area\n",
"box_pose.pose.position.z = 0.06\n",
"box_pose.pose.position.x = 0.15\n",
"box_pose.pose.orientation.w = 1\n",
"giskard.world.add_box(name=box_name, size=[0.05, 0.05, 0.15], pose=box_pose, parent_link='map')"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"Define a Pose to grasp the box from the top with the left gripper."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"goal_pose = PoseStamped()\n",
"goal_pose.header.frame_id = None\n",
"goal_pose.pose.position.x = None\n",
"goal_pose.pose.position.y = None\n",
"goal_pose.pose.position.z = None\n",
"goal_pose.pose.orientation = Quaternion(*quaternion_from_matrix([[0, 0, 0, 0],\n",
" [0, 0, 0, 0],\n",
" [0, 0, 0, 0],\n",
" [0, 0, 0, 0]]))\n",
"giskard.motion_goals.add_cartesian_pose(goal_pose=goal_pose, root_link='map', tip_link=left_gripper)\n",
"giskard.motion_goals.allow_all_collisions()\n",
"giskard.add_default_end_motion_conditions()\n",
"assert giskard.execute().error.code == GiskardError.SUCCESS"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"Define a Pose to move the base to while holding the box. the goal is to move the box away from the counter."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"giskard.world.update_parent_link_of_group(name=box_name, parent_link=left_gripper)\n",
"\n",
"drive_back_pose = PoseStamped()\n",
"drive_back_pose.header.frame_id = None\n",
"drive_back_pose.pose.position.x = None\n",
"drive_back_pose.pose.position.y = None\n",
"drive_back_pose.pose.position.z = None\n",
"drive_back_pose.pose.orientation = Quaternion(None)\n",
"\n",
"giskard.motion_goals.add_cartesian_pose(goal_pose=drive_back_pose, root_link='map',\n",
" tip_link='base_footprint')\n",
"giskard.motion_goals.allow_all_collisions()\n",
"giskard.add_default_end_motion_conditions()\n",
"assert giskard.execute().error.code == GiskardError.SUCCESS"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"Define a pose for the right gripper to grasp the handle of the fridge."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"grasp_handle_pose = PoseStamped()\n",
"grasp_handle_pose.header.frame_id = None\n",
"grasp_handle_pose.pose.orientation = Quaternion(None)\n",
"grasp_handle_pose.pose.position.x = None\n",
"grasp_handle_pose.pose.position.y = None\n",
"grasp_handle_pose.pose.position.z = None\n",
"\n",
"giskard.motion_goals.add_cartesian_pose(goal_pose=grasp_handle_pose, root_link='map',\n",
" tip_link=right_gripper)\n",
"giskard.motion_goals.allow_all_collisions()\n",
"giskard.add_default_end_motion_conditions()\n",
"assert giskard.execute().error.code == GiskardError.SUCCESS"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"The next cell automatically opens the fridge without any edits."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"giskard.motion_goals.add_open_container(tip_link=right_gripper,\n",
" environment_link=fridge_handle_name)\n",
"giskard.motion_goals.allow_all_collisions()\n",
"giskard.add_default_end_motion_conditions()\n",
"assert giskard.execute().error.code == GiskardError.SUCCESS"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"Define a pose in the fridge shelf to place the grasped box at and define a pose to keep the right gripper at the handle of the fridge."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"place_pose = PoseStamped()\n",
"place_pose.header.frame_id = None\n",
"place_pose.pose.position.x = None\n",
"place_pose.pose.position.y = None\n",
"place_pose.pose.position.z = None\n",
"place_pose.pose.orientation = Quaternion(None)\n",
"\n",
"giskard.motion_goals.add_cartesian_pose(goal_pose=place_pose,\n",
" root_link='map',\n",
" tip_link=box_name,\n",
" name='goal1')\n",
"\n",
"grasp_handle_pose = PoseStamped()\n",
"grasp_handle_pose.header.frame_id = None\n",
"grasp_handle_pose.pose.position.x = None\n",
"grasp_handle_pose.pose.position.y = None\n",
"grasp_handle_pose.pose.position.z = None\n",
"grasp_handle_pose.pose.orientation = Quaternion(None)\n",
"\n",
"giskard.motion_goals.add_cartesian_pose(goal_pose=grasp_handle_pose, root_link='map',\n",
" tip_link=right_gripper, name='goal2')\n",
"giskard.motion_goals.allow_all_collisions()\n",
"giskard.add_default_end_motion_conditions()\n",
"assert giskard.execute().error.code == GiskardError.SUCCESS"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
},
{
"cell_type": "markdown",
"source": [
"The next cell closes the fridge without any edits."
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%% md\n"
}
}
},
{
"cell_type": "code",
"execution_count": null,
"outputs": [],
"source": [
"giskard.world.update_parent_link_of_group(name=box_name, parent_link=fridge_shelf_name)\n",
"\n",
"giskard.motion_goals.add_close_container(tip_link=right_gripper,\n",
" environment_link=fridge_handle_name)\n",
"giskard.motion_goals.allow_all_collisions()\n",
"giskard.add_default_end_motion_conditions()\n",
"assert giskard.execute().error.code == GiskardError.SUCCESS"
],
"metadata": {
"collapsed": false,
"pycharm": {
"name": "#%%\n"
}
}
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}