{
"cells": [
{
"cell_type": "markdown",
"id": "c7167a5a-8ecb-4e68-b5ab-48f4d8948680",
"metadata": {
"execution": {
"iopub.execute_input": "2024-01-22T16:09:32.961531Z",
"iopub.status.busy": "2024-01-22T16:09:32.961000Z",
"iopub.status.idle": "2024-01-22T16:09:32.963371Z",
"shell.execute_reply": "2024-01-22T16:09:32.962935Z",
"shell.execute_reply.started": "2024-01-22T16:09:32.961514Z"
}
},
"source": [
"# Tracy Giskard Demo\n",
"\n",
"## 1. Launch Tracy simulation"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "39ed7604-0b98-4128-9029-81181f4485d5",
"metadata": {},
"outputs": [],
"source": [
"import rospy\n",
"import time\n",
"from utils import open_desktop, open_rvizweb, run_command, create_marker, publish_marker_array, TracyDemo\n",
"from std_msgs.msg import ColorRGBA\n",
"import random"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "8b4e8451-fdf1-4e5e-8054-468001cdf899",
"metadata": {},
"outputs": [],
"source": [
"# Open a desktop GUI or a Rvizweb on left sidecar\n",
"# Note: if you see connection error, try running this code cell again\n",
"# open_rvizweb()\n",
"open_desktop()"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "138373b3-704e-46c0-b8de-a4ee1c633536",
"metadata": {},
"outputs": [],
"source": [
"# Run the roslaunch in a background process\n",
"# Note: To see the logs, run this command in a terminal tab. \n",
"# Run this code cell will restart the simulation.\n",
"run_command('roslaunch giskardpy giskardpy_tracy_standalone.launch')"
]
},
{
"cell_type": "markdown",
"id": "c62dcdbd-f356-4ae0-b91c-fb70322e21b7",
"metadata": {},
"source": [
"## 2. Control the Tracy with the Giskard Python Interface"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "54711d4f-e950-4e36-86c0-2e05b13daa5a",
"metadata": {},
"outputs": [],
"source": [
"# initialize a ros node and the python wrapper for giskard\n",
"import rospy\n",
"from pprint import pprint\n",
"from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3Stamped, PointStamped\n",
"from giskardpy.python_interface.python_interface import GiskardWrapper\n",
"from giskardpy.goals.tracebot import InsertCylinder\n",
"\n",
"\n",
"rospy.init_node('gk_python_demo')\n",
"giskard_wrapper = GiskardWrapper()"
]
},
{
"cell_type": "markdown",
"id": "b8505a59-a976-4b0c-a94d-cd742f44a59b",
"metadata": {},
"source": [
"### List Joints and links"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "01514d5e-59e3-4b53-b278-3602b54ad0ae",
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"# List all the controlled joints\n",
"group_name = giskard_wrapper.world.get_group_names()[0]\n",
"robot_joints = giskard_wrapper.world.get_controlled_joints(group_name)\n",
"# List all the links\n",
"robot_links = giskard_wrapper.world.get_group_info(group_name).links\n",
"\n",
"print('Joints:')\n",
"pprint(robot_joints)\n",
"print('Links:')\n",
"pprint(robot_links)"
]
},
{
"cell_type": "markdown",
"id": "7f64a798-7542-4c2e-bd03-014882c86621",
"metadata": {},
"source": [
"### Define a default pose"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "25e1a6ef-3313-4105-90c6-7d20b57fbebc",
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"default_pose = {\n",
" 'left_shoulder_pan_joint': 2.539670467376709,\n",
" 'left_shoulder_lift_joint': -1.46823854119096,\n",
" 'left_elbow_joint': 2.1197431723224085,\n",
" 'left_wrist_1_joint': -1.4825000625899811,\n",
" 'left_wrist_2_joint': 5.467689037322998,\n",
" 'left_wrist_3_joint': -0.9808381239520472,\n",
" 'right_shoulder_pan_joint': 3.7588136196136475,\n",
" 'right_shoulder_lift_joint': -1.7489210567870082,\n",
" 'right_elbow_joint': -2.054229259490967,\n",
" 'right_wrist_1_joint': -1.6140786610045375,\n",
" 'right_wrist_2_joint': 0.7295855283737183,\n",
" 'right_wrist_3_joint': 3.944669485092163,\n",
"}\n",
"\n",
"# teleport giskard into the default pose (only works in simulation)\n",
"giskard_wrapper.monitors.add_set_seed_configuration(seed_configuration=default_pose)\n",
"giskard_wrapper.add_default_end_motion_conditions()\n",
"giskard_wrapper.execute()\n",
"\n",
"# move to default pose\n",
"def reset_to_default_pose():\n",
" giskard_wrapper.motion_goals.add_joint_position(goal_state=default_pose)\n",
" giskard_wrapper.motion_goals.allow_all_collisions()\n",
" giskard_wrapper.add_default_end_motion_conditions()\n",
" giskard_wrapper.execute()"
]
},
{
"cell_type": "markdown",
"id": "086d0523-47ce-4575-bf94-c72bf99a7445",
"metadata": {
"execution": {
"iopub.execute_input": "2024-06-12T10:02:31.179201Z",
"iopub.status.busy": "2024-06-12T10:02:31.178573Z",
"iopub.status.idle": "2024-06-12T10:02:31.747226Z",
"shell.execute_reply": "2024-06-12T10:02:31.745905Z",
"shell.execute_reply.started": "2024-06-12T10:02:31.179155Z"
},
"scrolled": true
},
"source": [
"### Spawn and attach cylinders to the hands"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "bb727b2f-3f56-4bad-b5cf-bf3da2a2128a",
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"cylinder_height = 0.121\n",
"cylinder_radius = 0.0225\n",
"\n",
"def spawn_cylinder(hand='r', color=(1, 1, 0, 1)):\n",
" cylinder_name = f'C_{time.time()}'\n",
" frame_id = f'{hand}_gripper_tool_frame'\n",
" pose = PoseStamped()\n",
" pose.header.frame_id = frame_id\n",
" pose.pose.position.z = cylinder_height / 5\n",
" pose.pose.orientation.w = 1\n",
" giskard_wrapper.world.add_cylinder(name=cylinder_name,\n",
" height=cylinder_height,\n",
" radius=cylinder_radius,\n",
" pose=pose,\n",
" parent_link=frame_id)\n",
" # dye it\n",
" giskard_wrapper.world.dye_group(cylinder_name, color)\n",
" return cylinder_name"
]
},
{
"cell_type": "markdown",
"id": "ce90d67c-c5e3-4a3e-98e1-867be42dcf9f",
"metadata": {},
"source": [
"### Call a goal to \"insert\" the cylinder at a point"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "cf34fbc3-522b-4a09-8f25-fa5de2adc82c",
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"def insert_cylider_to(x, y, cylinder_name):\n",
" hole_point = PointStamped()\n",
" hole_point.header.frame_id = 'table'\n",
" hole_point.point.x = x\n",
" hole_point.point.y = y\n",
" hole_point.point.z = 0.01\n",
" giskard_wrapper.motion_goals.add_motion_goal(motion_goal_class=InsertCylinder.__name__,\n",
" cylinder_name=cylinder_name,\n",
" cylinder_height=cylinder_height,\n",
" hole_point=hole_point)\n",
" giskard_wrapper.add_default_end_motion_conditions()\n",
" giskard_wrapper.execute()\n",
" # giskard_wrapper.world.remove_group(cylinder_name)\n",
" giskard_wrapper.world.detach_group(cylinder_name)\n",
" "
]
},
{
"cell_type": "markdown",
"id": "2c884f5c-12f6-4768-8ebf-91a9ab9297ba",
"metadata": {},
"source": [
"### Demo: Place the cylinder to block of the same color"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "5392a3de-77a3-43b8-a027-909f925a7626",
"metadata": {},
"outputs": [],
"source": [
"# Define 4 different colors\n",
"COLORS = [\n",
" ColorRGBA(0.259, 0.522, 0.957, 1.0), # Blue\n",
" ColorRGBA(0.984, 0.737, 0.02, 1.0), # Green\n",
" ColorRGBA(0.204, 0.659, 0.325, 1.0), # Yellow\n",
" ColorRGBA(0.918, 0.263, 0.208, 1.0), # Red\n",
"]\n",
"\n",
"# Initialize demo scenario\n",
"demo = TracyDemo(COLORS)\n",
"demo.setup_color_blocks()"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "159c1204-ce85-4734-8efd-0b7bac8e5537",
"metadata": {},
"outputs": [],
"source": [
"# Generate a random sample of colors\n",
"shuffled_color = random.choices(COLORS, k=10)\n",
"# shuffled_color = random.sample(COLORS, len(COLORS))"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "90bffbd9-64c4-4e67-885f-f2655f425e17",
"metadata": {},
"outputs": [],
"source": [
"# Spawn cylinders and place to target locations\n",
"for color in shuffled_color:\n",
" # randomly choose left or right hand\n",
" hand = random.choice(['r', 'l'])\n",
" # Spawn a cylinder in the choosen hand\n",
" cylinder_name = spawn_cylinder(color=(color.r, color.g, color.b, color.a), hand=hand)\n",
" rospy.sleep(1)\n",
" # Get the target location\n",
" target_pos = demo.get_color_pos(color=color, hand=hand)\n",
" # execute the motion\n",
" insert_cylider_to(target_pos.x, target_pos.y, cylinder_name)\n",
" reset_to_default_pose()"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "38b84017-2c5b-4fb3-9662-db898315b655",
"metadata": {},
"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.10.6"
}
},
"nbformat": 4,
"nbformat_minor": 5
}