From 48220a657a45eaf27e0096ab7ca083203a86ee95 Mon Sep 17 00:00:00 2001 From: malte Date: Tue, 20 Aug 2024 17:10:03 +0200 Subject: [PATCH] fixed wrong import --- scripts/giskard_examples.ipynb | 1468 ----------------- .../python_interface/python_interface.py | 2 +- 2 files changed, 1 insertion(+), 1469 deletions(-) delete mode 100644 scripts/giskard_examples.ipynb diff --git a/scripts/giskard_examples.ipynb b/scripts/giskard_examples.ipynb deleted file mode 100644 index 6083f8f14..000000000 --- a/scripts/giskard_examples.ipynb +++ /dev/null @@ -1,1468 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "# Section 0: Preliminaries\n", - "Please execute the following code blocks to setup the visualization, relevant imports, some helper functions and the robot selector." - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "%%bash --bg\n", - "rviz -d ../launch/../launch/rviz_config/standalone.rviz" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "is_executing": true, - "name": "#%%\n" - } - }, - "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 giskardpy.goals.weight_scaling_goals import MaxManipulabilityLinWeight, BaseArmWeightScaling\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 giskardpy.monitors.joint_monitors import JointGoalReached\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", - "from giskardpy.utils.utils import launch_launchfile\n", - "import subprocess\n", - "\n", - "# Define some helper functions\n", - "def reset_giskard():\n", - " gs.clear_motion_goals_and_monitors()\n", - " gs.world.clear()\n", - " if ROBOT == 'stretch':\n", - " default_pose = {\n", - " 'joint_gripper_finger_left': 0.6,\n", - " 'joint_gripper_finger_right': 0.6,\n", - " 'joint_right_wheel': 0.0,\n", - " 'joint_left_wheel': 0.0,\n", - " 'joint_lift': 0.5,\n", - " 'joint_arm_l3': 0.05,\n", - " 'joint_arm_l2': 0.05,\n", - " 'joint_arm_l1': 0.05,\n", - " 'joint_arm_l0': 0.05,\n", - " 'joint_wrist_yaw': 0.0,\n", - " 'joint_head_pan': 0.0,\n", - " 'joint_head_tilt': 0.0\n", - " }\n", - " elif 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 = gs.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 = gs.monitors.add_set_seed_odometry(base_pose=base_pose)\n", - " gs.motion_goals.allow_all_collisions()\n", - " gs.monitors.add_end_motion(start_condition=f'{done} and {done2}')\n", - " gs.execute()\n", - " gs.clear_motion_goals_and_monitors()\n", - "\n", - "\n", - "def visualize_last_task_graph():\n", - " folder_path = '../tmp/task_graphs'\n", - " files = glob.glob((os.path.join(folder_path, '*')))\n", - " latest_file = max(files, key=os.path.getmtime)\n", - " print(latest_file)\n", - " display(Image(filename=latest_file))\n", - "\n", - "\n", - "def visualize_last_gant_diagram():\n", - " folder_path = '../tmp/gantt_charts'\n", - " files = glob.glob((os.path.join(folder_path, '*')))\n", - " latest_file = max(files, key=os.path.getmtime)\n", - " images = convert_from_path(latest_file)\n", - " print(latest_file)\n", - " display(images[0])\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", - "\n", - "# List of available launch files\n", - "launch_files = {\n", - " 'PR2': 'package://giskardpy/launch/giskardpy_pr2_standalone_vrb.launch',\n", - " 'Stretch': 'package://giskardpy/launch/giskardpy_stretch_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", - "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", - " 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", - " 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", - "\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", - " launch_launchfile(selected_launch_file)\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')" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "# Section 1: Introduction\n", - "Giskard is a constraint- and optimization-based whole-body motion planning/control framework.\n", - "In short explain the QP...\n", - "Explain a constraint and how it influences the QP...\n", - "When using the python wrapper you do not specify constraints directly but rather a Motion Goal to parameterize a predefined set of constraints.\n", - "Additionally, the python interface allows to specify monitors that monitor a mathematical expression against a threshold and evaluate into a binary value.\n", - "They are used to start, stop or interrupt motion goals. More complex motion goals might specify their own monitors to chain constraints together.\n", - "Show picture of PR2 TF Tree...\n", - "\n", - "**Tutorial 1.1: Now initialize the Python Interface. It will connect to the running Giskard instance of your selected robot.**" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "from giskardpy.python_interface.python_interface import GiskardWrapper\n", - "gs = GiskardWrapper()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.2: Use the defined motion goal to move a single joint of the robot**\n", - "\n", - "The simplest possible motion goal call follows the schema:\n", - "\n", - " 1. Define a motion goal.\n", - " 2. Define a monitor that checks if the goal was reached.\n", - " 3. Define an end motion monitor, which has as start_condition the goal reached monitor." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "joint_goal = single_joint_goal\n", - "print(joint_goal)\n", - "joint_monitor = gs.monitors.add_joint_position(goal_state=joint_goal)\n", - "gs.motion_goals.add_joint_position(goal_state=joint_goal)\n", - "gs.monitors.add_end_motion(start_condition=joint_monitor)\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.3: Use the defined cartesian pose goal to move the tool frame of the robot**\n", - "\n", - "A commonly used goal is a cartesian pose goal. It is used to move a link of the robot to a specific pose. Frequently used links are the endeffector links or the base link.\n", - "\n", - "**Exercise 1.1: Change the orientation and position of the goal_pose and explore the generated movements**" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# Define a goal pose in the map coordinate system\n", - "goal_pose = PoseStamped()\n", - "goal_pose.header.frame_id = 'map'\n", - "goal_pose.pose.orientation = Quaternion(*quaternion_from_matrix([[1, 0, 0, 0],\n", - " [0, 1, 0, 0],\n", - " [0, 0, 1, 0],\n", - " [0, 0, 0, 1]]))\n", - "goal_pose.pose.position.x = 1.2\n", - "goal_pose.pose.position.y = -0.2\n", - "goal_pose.pose.position.z = 0.7\n", - "# specify the cartesian pose goal to move 'l_gripper_tool_frame' towards goal_pose by using the kinematic chain between 'l_gripper_tool_frame' and 'map'\n", - "gs.motion_goals.add_cartesian_pose(goal_pose, tool_frame, 'map')\n", - "# specify a monitor that is active when it is below the given thresholds and use it to end the motion\n", - "pose_monitor = gs.monitors.add_cartesian_pose('map', tool_frame, goal_pose, position_threshold=0.01,\n", - " orientation_threshold=0.01)\n", - "# add a monitor that automatically stops the motion when the trajectory is too long\n", - "gs.monitors.add_max_trajectory_length(30)\n", - "# use the pose monitor to end the motion when it is true\n", - "gs.monitors.add_end_motion(pose_monitor)\n", - "# in the case that there is no specific end motion condition the default end motion condition stops the motion when the robot stops moving i.e. the QP is in a local minimum.\n", - "# gs.add_default_end_motion_conditions()\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.4: Add a box to the world and specify a cartesian pose goal in its coordinate frame**\n", - "\n", - "One can also add different objects, meshes or URDF's to the world representation of Giskard.\n", - "When doing that, you can specify goal poses in the coordinate frames of the new entities.\n", - "\n", - "**Exercise 1.2: Change the box_pose and observe how giskard still reaches the same goal pose relative to the box**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "is_executing": true, - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# adding a box to the world\n", - "box_pose = PoseStamped()\n", - "box_pose.header.frame_id = 'map'\n", - "box_pose.pose.orientation.w = 1\n", - "box_pose.pose.position.x = 1.2\n", - "box_pose.pose.position.y = -0.2\n", - "box_pose.pose.position.z = 0.7\n", - "gs.world.add_box('mybox', size=(0.1, 0.05, 0.2), pose=box_pose, parent_link='map')\n", - "\n", - "# specfy a goal pose 8cm in front of the box origin frame\n", - "goal_pose = PoseStamped()\n", - "goal_pose.header.frame_id = 'mybox'\n", - "goal_pose.pose.orientation.w = 1\n", - "goal_pose.pose.position.x = -0.08\n", - "goal_pose.pose.position.y = 0\n", - "goal_pose.pose.position.z = 0\n", - "gs.motion_goals.add_cartesian_pose(goal_pose, tool_frame, 'map')\n", - "pose_monitor = gs.monitors.add_cartesian_pose('map', tool_frame, goal_pose, position_threshold=0.01,\n", - " orientation_threshold=0.01)\n", - "gs.monitors.add_end_motion(pose_monitor)\n", - "gs.monitors.add_max_trajectory_length(30)\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.5: Use sets of atomic constraints to specify a motion instead of using a full 6D-Pose**\n", - "\n", - "When using the cartesian pose goals all degrees of freedom of the link are fully constrained. One Advantage of constraint-based control is that one can only constrain necessary constraints.\n", - "The unconstrained DOF can then be exploited during the motion, for example in the collision avoidance or other secondary tasks.\n", - "To use this in the python interface one can use motion goals that constrain only a singular value, sometimes called constraints on feature functions ot atomic constraints.\n", - "For a feature function constraint a feature (Point or Vector) related to the robot (it has to be controllable) and one that is used as a reference has to be defined.\n", - "The name of the function then defines how they will be combined and 0 or 2 constraint values can be provided.\n", - "Right now we support the following atomic constraints:\n", - "\n", - " - Perpendicular(reference_vector, controlled_vector)\n", - " - Height(reference_point, controlled_point, lower_limit, upper_limit)\n", - " - Distance(reference_point, controlled_point, lower_limit, upper_limit)\n", - " - Pointing(reference_point, controlled_vector)\n", - " - Align(reference_vector, controlled_vector)\n", - " - Angle(reference_vector, controlled_vector, lower_limit, upper_limit)\n", - "\n", - "The controlled and reference features will be visualized as points or vectors in space.\n", - "\n", - "Here a distance constraint between the origin points of the tool link and the map coordinate frames is defined." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "# Define a world feature as a point at the origin of map\n", - "world_feature = PointStamped()\n", - "world_feature.header.frame_id = 'map'\n", - "\n", - "# Define a robot feature as a point at the origin of the right gripper tool frame\n", - "robot_feature = PointStamped()\n", - "robot_feature.header.frame_id = tool_frame\n", - "\n", - "# use the distance feature function to constrain the distance between the features to be between 2m and 2.5m\n", - "gs.motion_goals.add_distance(root_link='map',\n", - " tip_link=tool_frame,\n", - " reference_point=world_feature,\n", - " tip_point=robot_feature,\n", - " lower_limit=2,\n", - " upper_limit=2.5)\n", - "\n", - "gs.add_default_end_motion_conditions()\n", - "gs.monitors.add_max_trajectory_length(30)\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.6: Combine atomic constraints for grasping**\n", - "\n", - "We can exploit the rotational symmetry of a cylinder for grasping it if we specify the grasping motion with atomic constraints instead of with one fixed cartesian pose.\n", - "Here we use three different atomic constraints to achieve this.\n", - "\n", - "1. Pointing the gripper towards the object center\n", - "2. keeping a suitable distance\n", - "3. constrain the height to be within the dimensions of the objects\n", - "\n", - "**Excercise 1.3: Change the position of the cylinder and the constraint values, and observe the resulting grasp poses.**\n", - "\n", - "**Excercise 1.4: You might notice that at some positions the rotation of the gripper is not correct. This is due to a missing atomic constraint that allows the gripper_frame to freely rotate around its x-axis. Try to constrain this using the AlignFeatureFunction. You can visualize tf in rviz to see all coordinate frames**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "is_executing": true, - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# variables\n", - "gripper_link = tool_frame\n", - "\n", - "# adding the object\n", - "cyl_pose = PoseStamped()\n", - "cyl_pose.header.frame_id = 'map'\n", - "cyl_pose.pose.orientation.w = 1\n", - "cyl_pose.pose.position = Point(1.2, -0.2, 1.2)\n", - "gs.world.add_cylinder('mycyl', height=0.2, radius=0.02, pose=cyl_pose, parent_link='map')\n", - "\n", - "# define the robot features\n", - "robot_pointing_feature = Vector3Stamped()\n", - "robot_pointing_feature.header.frame_id = gripper_link\n", - "robot_pointing_feature.vector = Vector3(1, 0, 0)\n", - "\n", - "robot_point_feature = PointStamped()\n", - "robot_point_feature.header.frame_id = gripper_link\n", - "robot_point_feature.point = Point(0, 0, 0)\n", - "\n", - "# define the world features\n", - "world_cylinder_center_feature = PointStamped()\n", - "world_cylinder_center_feature.header.frame_id = 'mycyl'\n", - "world_cylinder_center_feature.point = Point(0, 0, 0)\n", - "\n", - "# define the constraints\n", - "gs.motion_goals.add_distance(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=0.03,\n", - " upper_limit=0.03)\n", - "gs.motion_goals.add_height(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=-0.05,\n", - " upper_limit=0.05)\n", - "gs.motion_goals.add_pointing(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_point=world_cylinder_center_feature,\n", - " pointing_axis=robot_pointing_feature)\n", - "\n", - "#Exercise 1.4: You can define your features and your motion goal here\n", - "\n", - "gs.add_default_end_motion_conditions()\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.7:Adding collision avoidance**\n", - "\n", - "Another advantage of having unconstrained DOF in the motion specification can be seen if we put an obstacle in front of the object we want to grasp and configure the collision avoidance to avoid this obstacle with all parts of the robot.\n", - "\n", - "**Excercise 1.5: change the position and the size of the obstacle and observe the resulting motions**\n", - "PS: this cell includes the solution to the previous exercise" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# variables\n", - "gripper_link = tool_frame\n", - "\n", - "# adding the object\n", - "cyl_pose = PoseStamped()\n", - "cyl_pose.header.frame_id = 'map'\n", - "cyl_pose.pose.orientation.w = 1\n", - "cyl_pose.pose.position = Point(1.2, -0.2, 1.2)\n", - "gs.world.add_cylinder('mycyl', height=0.2, radius=0.02, pose=cyl_pose, parent_link='map')\n", - "\n", - "obstacle_pose = PoseStamped()\n", - "obstacle_pose.header.frame_id = 'mycyl'\n", - "obstacle_pose.pose.orientation.w = 1\n", - "obstacle_pose.pose.position = Point(-0.1, 0.1, 0)\n", - "gs.world.add_box('obstacle', size=(0.02, 0.06, 0.05), pose=obstacle_pose, parent_link='mycyl')\n", - "\n", - "# define the robot features\n", - "robot_pointing_feature = Vector3Stamped()\n", - "robot_pointing_feature.header.frame_id = gripper_link\n", - "robot_pointing_feature.vector = Vector3(1, 0, 0)\n", - "\n", - "robot_point_feature = PointStamped()\n", - "robot_point_feature.header.frame_id = gripper_link\n", - "robot_point_feature.point = Point(0, 0, 0)\n", - "\n", - "robot_gripper_z_axis_feature = Vector3Stamped()\n", - "robot_gripper_z_axis_feature.header.frame_id = gripper_link\n", - "robot_gripper_z_axis_feature.vector = Vector3(0, 0, 1)\n", - "\n", - "# define the world features\n", - "world_cylinder_center_feature = PointStamped()\n", - "world_cylinder_center_feature.header.frame_id = 'mycyl'\n", - "world_cylinder_center_feature.point = Point(0, 0, 0)\n", - "\n", - "world_cyl_z_axis_feature = Vector3Stamped()\n", - "world_cyl_z_axis_feature.header.frame_id = 'mycyl'\n", - "world_cyl_z_axis_feature.vector = Vector3(0, 0, 1)\n", - "\n", - "# define the constraints\n", - "gs.motion_goals.add_distance(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=0.03,\n", - " upper_limit=0.03)\n", - "gs.motion_goals.add_height(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=-0.05,\n", - " upper_limit=0.05)\n", - "gs.motion_goals.add_pointing(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_point=world_cylinder_center_feature,\n", - " pointing_axis=robot_pointing_feature)\n", - "gs.motion_goals.avoid_collision(min_distance=0.1, group1='obstacle')\n", - "gs.motion_goals.add_align_planes(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_normal=world_cyl_z_axis_feature,\n", - " tip_normal=robot_gripper_z_axis_feature)\n", - "gs.add_default_end_motion_conditions()\n", - "assert gs.execute().error.code == GiskardError.SUCCESS\n", - "\n", - "# EXERCISE:\n", - "# 1. Change Pose and size of the obstacle and see of the robot behaves." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.8: Use monitors to evaluate the success of your specified motion**\n", - "\n", - "For each constraint a related monitor can be added to observe their progression and collect feedback in the case of failure where one or many constraints cannot be satisfied.\n", - "A Motion was not sucessfull if the end motion monitor was not active and the goal was canceled by another monitor. In that case the return value of the execute() call can be passed to the get_end_motion_reason() function to obtain a summary of all monitors that are connected to the end motion that are not active.\n", - "\n", - "**Excercise 1.6: The output tells you which monitor of the end motion was not successful. Try changing/removing/adding constraint and monitors to realize a successful execution.**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "is_executing": true, - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# variables\n", - "gripper_link = tool_frame\n", - "\n", - "# adding the object\n", - "cyl_pose = PoseStamped()\n", - "cyl_pose.header.frame_id = 'map'\n", - "cyl_pose.pose.orientation.w = 1\n", - "cyl_pose.pose.position = Point(1.2, -0.2, 1.2)\n", - "gs.world.add_cylinder('mycyl', height=0.2, radius=0.02, pose=cyl_pose, parent_link='map')\n", - "\n", - "obstacle_pose = PoseStamped()\n", - "obstacle_pose.header.frame_id = 'mycyl'\n", - "obstacle_pose.pose.orientation.w = 1\n", - "obstacle_pose.pose.position = Point(-0.1, 0.1, 0)\n", - "gs.world.add_box('obstacle', size=(0.02, 0.06, 0.05), pose=obstacle_pose, parent_link='mycyl')\n", - "\n", - "# define the robot features\n", - "robot_pointing_feature = Vector3Stamped()\n", - "robot_pointing_feature.header.frame_id = gripper_link\n", - "robot_pointing_feature.vector = Vector3(1, 0, 0)\n", - "\n", - "robot_point_feature = PointStamped()\n", - "robot_point_feature.header.frame_id = gripper_link\n", - "robot_point_feature.point = Point(0, 0, 0)\n", - "\n", - "robot_gripper_z_axis_feature = Vector3Stamped()\n", - "robot_gripper_z_axis_feature.header.frame_id = gripper_link\n", - "robot_gripper_z_axis_feature.vector = Vector3(0, 0, 1)\n", - "\n", - "# define the world features\n", - "world_cylinder_center_feature = PointStamped()\n", - "world_cylinder_center_feature.header.frame_id = 'mycyl'\n", - "world_cylinder_center_feature.point = Point(0, 0, 0)\n", - "\n", - "world_cyl_z_axis_feature = Vector3Stamped()\n", - "world_cyl_z_axis_feature.header.frame_id = 'mycyl'\n", - "world_cyl_z_axis_feature.vector = Vector3(0, 0, 1)\n", - "\n", - "# define the constraints\n", - "gs.motion_goals.add_distance(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=0.0,\n", - " upper_limit=0.0)\n", - "mon_dist = gs.monitors.add_distance(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=-0.001,\n", - " upper_limit=0.001)\n", - "\n", - "gs.motion_goals.add_height(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=-0.05,\n", - " upper_limit=0.05)\n", - "mon_height = gs.monitors.add_height(root_link='map',\n", - " tip_link=gripper_link,\n", - " reference_point=world_cylinder_center_feature,\n", - " tip_point=robot_point_feature,\n", - " lower_limit=-0.05,\n", - " upper_limit=0.05)\n", - "\n", - "gs.motion_goals.add_pointing(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_point=world_cylinder_center_feature,\n", - " pointing_axis=robot_pointing_feature)\n", - "mon_pointing = gs.monitors.add_pointing_at(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_point=world_cylinder_center_feature,\n", - " pointing_axis=robot_pointing_feature)\n", - "\n", - "gs.motion_goals.add_align_planes(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_normal=world_cyl_z_axis_feature,\n", - " tip_normal=robot_gripper_z_axis_feature)\n", - "mon_align = gs.monitors.add_vectors_aligned(root_link='map',\n", - " tip_link=gripper_link,\n", - " goal_normal=world_cyl_z_axis_feature,\n", - " tip_normal=robot_gripper_z_axis_feature)\n", - "\n", - "gs.motion_goals.allow_all_collisions()\n", - "gs.motion_goals.avoid_collision(min_distance=0.08, group1='obstacle')\n", - "gs.monitors.add_end_motion(f'{mon_dist} and {mon_height} and {mon_align} and {mon_pointing}')\n", - "gs.monitors.add_cancel_motion(gs.monitors.add_local_minimum_reached(),\n", - " error_message='local minimum reached while monitors are not satisfied')\n", - "result = gs.execute()\n", - "if result.error.code != GiskardError.SUCCESS:\n", - " print(gs.get_end_motion_reason(move_result=result, show_all=False)) " - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 1.9: Beyond constraints. Adapting the cost function to change behaviour.**\n", - "\n", - "The cost function of Giskard consists of a quadratic and a linear term. The quadartic term weights all control, slack variables against each other user using a weight matrix. Normally all control variables realted to joints a weight the same. With the BaseArmWeightScaling Goal we can define to joint groups (base joints and arm joints) and change their weights depending on the euclidean distance between the defined tip link and its goal position. Doing this the base joints will be preffered to satisfy all other goals/constraints when the distance is larger. When the distance becomes smaller the base joints will be used less and the arm joints will be used more. But there is nor hard cut, meaning the base joints can also used when distance is small if it is needed.\n", - "\n", - "The linear weight is normally not used, but by using the MaxManipulabilityLinWeight Goal a linear weight term is created that maximiyes the manipulability of the kinematic chain whic is defined by the root link and tip link parameters.\n", - "\n", - "In this tutorial we use the presented goals to show a combination of constraints and cost function augmentations to:\n", - "- place both grippers at specific poses (2 6D Pose Constraints)\n", - "- keep the line of sight on the left gripper (Pointing Constraint)\n", - "- prefer using base movements if the grippers are further away from their goal poses and avoid base movements if they are close to their goals (Quadratic Weight Scaling)\n", - "- maximize the manipulability in each arm (Linear weight term in the cost function)\n", - "\n", - "**Exercise 1.6: Comment out the goals for the cost function augmentation to compare the crated motions with and without**" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "# This is eventually not feasible for the stretch\n", - "if ROBOT == 'pr2':\n", - " js = {\n", - " # 'torso_lift_joint': 0.2999225173357618,\n", - " 'head_pan_joint': 0.041880780651479044,\n", - " 'head_tilt_joint': -0.37,\n", - " 'r_upper_arm_roll_joint': -0.9487714747527726,\n", - " 'r_shoulder_pan_joint': -1.0047307505973626,\n", - " 'r_shoulder_lift_joint': 0.48736790658811985,\n", - " 'r_forearm_roll_joint': -14.895833882874182,\n", - " 'r_elbow_flex_joint': -1.392377908925028,\n", - " 'r_wrist_flex_joint': -0.4548695149411013,\n", - " 'r_wrist_roll_joint': 0.11426798984097819,\n", - " 'l_upper_arm_roll_joint': 1.7383062350263658,\n", - " 'l_shoulder_pan_joint': 1.8799810286792007,\n", - " 'l_shoulder_lift_joint': 0.011627231224188975,\n", - " 'l_forearm_roll_joint': 312.67276414458695,\n", - " 'l_elbow_flex_joint': -2.0300928925694675,\n", - " 'l_wrist_flex_joint': -0.10014623223021513,\n", - " 'l_wrist_roll_joint': -6.062015047706399,\n", - " }\n", - " gs.motion_goals.add_joint_position(js)\n", - " gs.motion_goals.allow_all_collisions()\n", - " gs.add_default_end_motion_conditions()\n", - " gs.execute()\n", - "\n", - "goal_pose = PoseStamped()\n", - "goal_pose.header.frame_id = 'map'\n", - "goal_pose.pose.orientation = Quaternion(*quaternion_from_matrix([[1, 0, 0, 0],\n", - " [0, 1, 0, 0],\n", - " [0, 0, 1, 0],\n", - " [0, 0, 0, 1]]))\n", - "goal_pose.pose.position.x = 2.01\n", - "goal_pose.pose.position.y = -0.2\n", - "goal_pose.pose.position.z = 0.7\n", - "\n", - "goal_pose2 = deepcopy(goal_pose)\n", - "goal_pose2.pose.position.y = -0.6\n", - "goal_pose2.pose.position.z = 0.8\n", - "goal_pose2.pose.orientation = Quaternion(*quaternion_from_matrix([[0, 0, 1, 0],\n", - " [0, 1, 0, 0],\n", - " [-1, 0, 0, 0],\n", - " [0, 0, 0, 1]]))\n", - "\n", - "gs.motion_goals.add_cartesian_pose(goal_pose, tool_frame, 'map')\n", - "if ROBOT == 'pr2':\n", - " gs.motion_goals.add_cartesian_pose(goal_pose2, 'r_gripper_tool_frame', 'map', name='cart2')\n", - "\n", - "goal_point = PointStamped()\n", - "goal_point.header.frame_id = goal_pose.header.frame_id\n", - "goal_point.point = goal_pose.pose.position\n", - "pointing_axis = Vector3Stamped()\n", - "pointing_axis.header.frame_id = cam_frame\n", - "pointing_axis.vector.z = 1\n", - "gs.motion_goals.add_pointing(goal_point, cam_frame, pointing_axis, 'map')\n", - "\n", - "x_base = Vector3Stamped()\n", - "x_base.header.frame_id = 'base_link'\n", - "x_base.vector.x = 1\n", - "x_goal = Vector3Stamped()\n", - "x_goal.header.frame_id = 'map'\n", - "x_goal.vector.x = 1\n", - "gs.motion_goals.add_align_planes(tip_link='base_link',\n", - " root_link='map',\n", - " tip_normal=x_base,\n", - " goal_normal=x_goal)\n", - "\n", - "if ROBOT == 'pr2':\n", - " arm_joints = [\n", - " 'torso_lift_joint',\n", - " # 'head_pan_joint',\n", - " # 'head_tilt_joint',\n", - " 'r_upper_arm_roll_joint',\n", - " 'r_shoulder_pan_joint',\n", - " 'r_shoulder_lift_joint',\n", - " 'r_forearm_roll_joint',\n", - " 'r_elbow_flex_joint',\n", - " 'r_wrist_flex_joint',\n", - " 'r_wrist_roll_joint',\n", - " 'l_upper_arm_roll_joint',\n", - " 'l_shoulder_pan_joint',\n", - " 'l_shoulder_lift_joint',\n", - " 'l_forearm_roll_joint',\n", - " 'l_elbow_flex_joint',\n", - " 'l_wrist_flex_joint',\n", - " 'l_wrist_roll_joint']\n", - " gain = 100000\n", - "elif ROBOT == 'stretch':\n", - " arm_joints = ['joint_arm_l3',\n", - " 'joint_arm_l2',\n", - " 'joint_arm_l1',\n", - " 'joint_arm_l0',\n", - " 'joint_wrist_yaw',\n", - " 'joint_wrist_pitch',\n", - " 'joint_wrist_roll']\n", - " gain = 100000\n", - "\n", - "tip_goal = PointStamped()\n", - "tip_goal.header.frame_id = 'map'\n", - "tip_goal.point = goal_pose.pose.position\n", - "gs.motion_goals.add_motion_goal(motion_goal_class=BaseArmWeightScaling.__name__,\n", - " root_link='map',\n", - " tip_link=tool_frame,\n", - " tip_goal=tip_goal,\n", - " gain=gain,\n", - " arm_joints=arm_joints,\n", - " base_joints=['brumbrum'])\n", - "if ROBOT == 'pr2':\n", - " gs.motion_goals.add_motion_goal(motion_goal_class=MaxManipulabilityLinWeight.__name__,\n", - " root_link='torso_lift_link',\n", - " tip_link='r_gripper_tool_frame')\n", - " gs.motion_goals.add_motion_goal(motion_goal_class=MaxManipulabilityLinWeight.__name__,\n", - " root_link='torso_lift_link',\n", - " tip_link='l_gripper_tool_frame',\n", - " name='MaxMal2')\n", - "gs.add_default_end_motion_conditions()\n", - "gs.motion_goals.allow_all_collisions()\n", - "assert gs.execute().error.code == GiskardError.SUCCESS\n", - "\n", - "# EXERCISE:\n", - "# 1.Comment the BaseArmWeightScaling and MaxManipulabilityLinWeight Goals to observe the resulting motions when the cost function is not augmented." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "# Section 2: Environment Manipulation\n", - "**Tutorial 2.1: Opening a dishwasher**\n", - "\n", - "If an environment is modelled as articulated links in an URDF Giskard can load it and control the joint states of the environment when it is connected to links of the robot.\n", - "Here we had already loaded a URDF description of the iai_kitchen onto the parameter server. WE now read this from the parameter server to add it to the world model of Giskard.\n", - "\n", - "Then we grasp the dishwasher handle and open the dishwasher by defining a joint goal for its door joint.\n", - "the open_container goal adds constraints to keep the robot gripper at the same pose relative to the door handle. This then causes the whole robot to move when the movements of the dishwasher joint are calculated to achieve the joint goal.\n", - "\n", - "**Exercise 2.1: try different joint goals**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n", - "is_executing": true - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# load the kitchen environment\n", - "kitchen_pose = PoseStamped()\n", - "kitchen_pose.header.frame_id = 'map'\n", - "kitchen_pose.pose.orientation.w = 1\n", - "gs.world.add_urdf(name='iai_kitchen',\n", - " urdf=rospy.get_param('kitchen_description'),\n", - " pose=kitchen_pose)\n", - "\n", - "# Define poses and groups\n", - "p = PoseStamped()\n", - "p.header.frame_id = 'map'\n", - "p.pose.orientation.w = 1\n", - "p.pose.position.x = 0.5\n", - "p.pose.position.y = 0.2\n", - "\n", - "hand = tool_frame\n", - "\n", - "goal_angle = np.pi / 4\n", - "handle_frame_id = 'sink_area_dish_washer_door_handle'\n", - "handle_name = 'sink_area_dish_washer_door_handle'\n", - "gs.world.register_group('dishwasher', root_link_name='sink_area_dish_washer_main',\n", - " root_link_group_name='iai_kitchen')\n", - "gs.world.register_group('handle', root_link_name=handle_name,\n", - " root_link_group_name='iai_kitchen')\n", - "\n", - "# grasp the dishwasher handle\n", - "bar_axis = Vector3Stamped()\n", - "bar_axis.header.frame_id = handle_frame_id\n", - "bar_axis.vector.y = 1\n", - "\n", - "bar_center = PointStamped()\n", - "bar_center.header.frame_id = handle_frame_id\n", - "\n", - "tip_grasp_axis = Vector3Stamped()\n", - "tip_grasp_axis.header.frame_id = hand\n", - "tip_grasp_axis.vector.z = 1\n", - "\n", - "gs.motion_goals.add_grasp_bar(root_link='map',\n", - " tip_link=hand,\n", - " tip_grasp_axis=tip_grasp_axis,\n", - " bar_center=bar_center,\n", - " bar_axis=bar_axis,\n", - " bar_length=.3)\n", - "\n", - "x_gripper = Vector3Stamped()\n", - "x_gripper.header.frame_id = hand\n", - "x_gripper.vector.x = 1\n", - "\n", - "x_goal = Vector3Stamped()\n", - "x_goal.header.frame_id = handle_frame_id\n", - "x_goal.vector.x = -1\n", - "gs.motion_goals.add_align_planes(tip_link=hand,\n", - " root_link='map',\n", - " tip_normal=x_gripper,\n", - " goal_normal=x_goal)\n", - "\n", - "if ROBOT == 'stretch':\n", - " x_base = Vector3Stamped()\n", - " x_base.header.frame_id = 'base_link'\n", - " x_base.vector.x = 1\n", - " x_goal = Vector3Stamped()\n", - " x_goal.header.frame_id = 'map'\n", - " x_goal.vector.y = 1\n", - " gs.motion_goals.add_align_planes(tip_link='base_link',\n", - " root_link='map',\n", - " tip_normal=x_base,\n", - " goal_normal=x_goal,\n", - " name='align_base')\n", - "\n", - "gs.add_default_end_motion_conditions()\n", - "gs.execute()\n", - "\n", - "# open the dishwasher\n", - "gs.motion_goals.add_open_container(tip_link=hand,\n", - " environment_link=handle_name,\n", - " goal_joint_state=goal_angle)\n", - "gs.motion_goals.allow_all_collisions()\n", - "gs.add_default_end_motion_conditions()\n", - "gs.execute()\n", - "\n", - "# close the dishwasher\n", - "gs.motion_goals.add_open_container(tip_link=hand,\n", - " environment_link=handle_name,\n", - " goal_joint_state=0)\n", - "gs.motion_goals.allow_all_collisions()\n", - "gs.add_default_end_motion_conditions()\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "# Section 3: Monitors and Giskard Task Graphs\n", - "**Tutorial 3.1: Simple sequencing of to cartesian pose goals in one controller**\n", - "\n", - "Until now, we have seen different motion goals and monitors to observe the success of the specified Motion Goals, i.e. constraints.\n", - "Another way to specify a motion with giskard is to utilize monitors and the start-, to_hold- and end_condition's each motion goal and monitor offer.\n", - "Doing this, a chain, or more specifically a graph (task graph in Giskard terms), of motion goals is build up.\n", - "The task graph will be executed as one continuous controller (no need to recompile a new one for each goal).\n", - "After execution a picture of the task graph and a Gant diagram visualizing the execution order of goals and monitors can be inspected.\n", - "Next we will see simple example to execute two cartesian poses after each other." - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "pose1 = PoseStamped()\n", - "pose1.header.frame_id = 'map'\n", - "pose1.pose.position.x = 1\n", - "pose1.pose.orientation.w = 1\n", - "\n", - "pose2 = PoseStamped()\n", - "pose2.header.frame_id = base_link\n", - "pose2.pose.position.y = 1\n", - "pose2.pose.orientation.w = 1\n", - "\n", - "root_link = 'map'\n", - "tip_link = base_link\n", - "\n", - "monitor1 = gs.monitors.add_cartesian_pose(name='pose1',\n", - " root_link=root_link,\n", - " tip_link=tip_link,\n", - " goal_pose=pose1)\n", - "\n", - "monitor2 = gs.monitors.add_cartesian_pose(name='pose2',\n", - " root_link=root_link,\n", - " tip_link=tip_link,\n", - " goal_pose=pose2,\n", - " absolute=True,\n", - " start_condition=monitor1)\n", - "end_monitor = gs.monitors.add_local_minimum_reached()\n", - "\n", - "gs.motion_goals.add_cartesian_pose(goal_pose=pose1,\n", - " name='g1',\n", - " root_link=root_link,\n", - " tip_link=tip_link,\n", - " end_condition=monitor1)\n", - "gs.motion_goals.add_cartesian_pose(goal_pose=pose2,\n", - " name='g2',\n", - " root_link=root_link,\n", - " tip_link=tip_link,\n", - " absolute=True,\n", - " start_condition=monitor1,\n", - " end_condition=f'{monitor2} and {end_monitor}')\n", - "gs.motion_goals.allow_all_collisions()\n", - "gs.monitors.add_end_motion(start_condition=' and '.join([end_monitor, monitor2]))\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Visualize the Task Graph**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "visualize_last_task_graph()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Visualize the Gant diagramm**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "visualize_last_gant_diagram()" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%% md\n" - } - }, - "source": [ - "**Tutorial 3.2: Complex Sequencing**\n", - "\n", - "This can be utilized to build arbitrary complex task graphs.\n", - "\n", - "**Excersise 3.1: Read the code and compare your expectations with the execution, the printed out task graph and the Gantt diagramm.**\n", - "\n", - "**Excercise 3.2: Come up with your own sequences using everything we have looked at so far**" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n", - "is_executing": true - } - }, - "outputs": [], - "source": [ - "reset_giskard()\n", - "\n", - "# %% Define goals for later\n", - "right_arm_goal = {'r_shoulder_pan_joint': -1.7125,\n", - " 'r_shoulder_lift_joint': -0.25672,\n", - " 'r_upper_arm_roll_joint': -1.46335,\n", - " 'r_elbow_flex_joint': -2.12,\n", - " 'r_forearm_roll_joint': 1.76632,\n", - " 'r_wrist_flex_joint': -0.10001,\n", - " 'r_wrist_roll_joint': 0.05106}\n", - "\n", - "left_arm_goal = {'l_shoulder_pan_joint': 1.9652,\n", - " 'l_shoulder_lift_joint': - 0.26499,\n", - " 'l_upper_arm_roll_joint': 1.3837,\n", - " 'l_elbow_flex_joint': -2.12,\n", - " 'l_forearm_roll_joint': 16.99,\n", - " 'l_wrist_flex_joint': - 0.10001,\n", - " 'l_wrist_roll_joint': 0}\n", - "\n", - "base_goal = PoseStamped()\n", - "base_goal.header.frame_id = 'map'\n", - "base_goal.pose.position.x = 2\n", - "base_goal.pose.orientation.w = 1\n", - "\n", - "# %% Monitors observe something and turn to True, if the condition is met. They don't cause any motions.\n", - "# All monitor related operations are grouped under giskard_wrapper.monitors.\n", - "# Let's define a few\n", - "# This one turns True when the length of the current trajectory % mod = 0\n", - "alternator = gs.monitors.add_alternator(mod=2)\n", - "# This one sleeps and then turns True\n", - "sleep1 = gs.monitors.add_sleep(1, name='sleep1')\n", - "# This prints a message and then turns True.\n", - "# With start_condition you can define which monitors need to be True in order for this one to become active\n", - "print1 = gs.monitors.add_print(message=f'{sleep1} done', start_condition=sleep1)\n", - "# You can also write logical expressions using \"and\", \"or\" and \"not\" to combine multiple monitors\n", - "sleep2 = gs.monitors.add_sleep(1.5, name='sleep2', start_condition=f'{print1} or not {sleep1}')\n", - "\n", - "# %% Now Let's define some motion goals.\n", - "# We want to reach two joint goals, so we first define monitors for checking that end condition.\n", - "right_monitor = gs.monitors.add_joint_position(goal_state=right_arm_goal,\n", - " name='right pose reached',\n", - " start_condition=sleep1)\n", - "# You can use add_motion_goal to add any monitor implemented in giskardpy.monitor.\n", - "# All remaining parameters are forwarded to the __init__ function of that class.\n", - "# All specialized add_ functions are just wrappers for add_monitor.\n", - "left_monitor = gs.monitors.add_monitor(monitor_class=JointGoalReached.__name__,\n", - " goal_state=left_arm_goal,\n", - " name='left pose reached',\n", - " start_condition=sleep1,\n", - " threshold=0.01)\n", - "\n", - "# We set two separate motion goals for the joints of the left and right arm.\n", - "# All motion goal related operations are groups under gs.motion_goals.\n", - "# The one for the right arm starts when the sleep2 monitor is done and ends, when the right_monitor is done,\n", - "# meaning it continues until the joint goal was reached.\n", - "gs.motion_goals.add_joint_position(goal_state=right_arm_goal,\n", - " name='right pose',\n", - " start_condition=sleep2,\n", - " end_condition=right_monitor)\n", - "# You can use add_motion_goal to add any motion goal implemented in giskardpy.goals.\n", - "# All remaining parameters are forwarded to the __init__ function of that class.\n", - "gs.motion_goals.add_motion_goal(motion_goal_class=JointPositionList.__name__,\n", - " goal_state=left_arm_goal,\n", - " name='left pose',\n", - " end_condition=left_monitor)\n", - "\n", - "# %% Now let's define a goal for the base, 2m in front of it.\n", - "# First we define a monitor which checks if that pose was reached.\n", - "base_monitor = gs.monitors.add_cartesian_pose(root_link='map',\n", - " tip_link='base_footprint',\n", - " goal_pose=base_goal)\n", - "\n", - "# and then we define a motion goal for it.\n", - "# The hold_condition causes the motion goal to hold as long as the condition is True.\n", - "# In this case, the cart pose is halted if time % 2 == 1 and active if time % 2 == 0.\n", - "gs.motion_goals.add_cartesian_pose(root_link='map',\n", - " tip_link='base_footprint',\n", - " goal_pose=base_goal,\n", - " hold_condition=f'not {alternator}',\n", - " end_condition=base_monitor)\n", - "\n", - "# %% Define when the motion should end.\n", - "# Usually you'd use the local minimum reached monitor for this.\n", - "# Most monitors also have a stay_true parameter (when it makes sense), with reasonable default values.\n", - "# In this case, we don't want the local minimum reached monitor to stay True, because it might get triggered during\n", - "# the sleeps and therefore set it to False.\n", - "local_min = gs.monitors.add_local_minimum_reached(stay_true=False)\n", - "\n", - "# Giskard will only end the motion generation and return Success, if an end monitor becomes True.\n", - "# We do this by defining one that gets triggered, when a local minimum was reached, sleep2 is done and the motion goals\n", - "# were reached.\n", - "gs.monitors.add_end_motion(start_condition=' and '.join([local_min,\n", - " sleep2,\n", - " right_monitor,\n", - " left_monitor,\n", - " base_monitor]))\n", - "# It's good to also add a cancel condition in case something went wrong and the end motion monitor is unable to become\n", - "# True. Currently, the only predefined specialized cancel monitor is max trajectory length.\n", - "# Alternative you can use monitor.add_cancel_motion similar to end_motion.\n", - "gs.monitors.add_max_trajectory_length(120)\n", - "# Lastly we allow all collisions\n", - "gs.motion_goals.allow_all_collisions()\n", - "# And execute the goal.\n", - "assert gs.execute().error.code == GiskardError.SUCCESS" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "visualize_last_task_graph()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [ - "visualize_last_gant_diagram()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "collapsed": false, - "jupyter": { - "outputs_hidden": false - }, - "pycharm": { - "name": "#%%\n" - } - }, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} \ No newline at end of file diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index d0bdd7f0c..e71387a09 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -40,7 +40,7 @@ PayloadAlternator from giskardpy.utils.utils import kwargs_to_json, get_all_classes_in_package from giskardpy.goals.feature_functions import AlignPerpendicular, HeightGoal, AngleGoal, DistanceGoal -from giskardpy.monitors.feature_monitors import PerpendicularMonitor, AngleMonitor, HeightMonitor, DistanceMonitor +from giskardpy.motion_graph.monitors.feature_monitors import PerpendicularMonitor, AngleMonitor, HeightMonitor, DistanceMonitor from giskard_msgs.msg import ExecutionState