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