{
"cells": [
{
"cell_type": "code",
"execution_count": null,
"id": "initial_id",
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"%%bash\n",
"cd /home/jovyan/workspace/ros/src/giskardpy_ros/ && git checkout c4a25152faa05246a4c263b22b76696b7bc6a72a"
]
},
{
"metadata": {},
"cell_type": "code",
"outputs": [],
"execution_count": null,
"source": [
"%%bash --bg\n",
"roslaunch giskardpy_ros giskardpy_justin_standalone.launch"
],
"id": "e10f2a7e539caebe"
},
{
"metadata": {},
"cell_type": "code",
"outputs": [],
"execution_count": null,
"source": [
"import rospy\n",
"from geometry_msgs.msg import PoseStamped, Quaternion\n",
"import numpy as np\n",
"from giskardpy.utils.math import quaternion_from_axis_angle\n",
"from giskardpy_ros.python_interface.python_interface import GiskardWrapper\n",
"from copy import deepcopy"
],
"id": "48bdd594f165488f"
},
{
"metadata": {},
"cell_type": "code",
"outputs": [],
"execution_count": null,
"source": "rospy.init_node('justin_demo')",
"id": "1f4e1e15d4d406e0"
},
{
"metadata": {},
"cell_type": "code",
"outputs": [],
"execution_count": null,
"source": "giskard = GiskardWrapper()",
"id": "5e93e2c3e069b1a9"
},
{
"metadata": {},
"cell_type": "code",
"outputs": [],
"execution_count": null,
"source": [
"kitchen_pose = PoseStamped()\n",
"kitchen_pose.header.frame_id = 'map'\n",
"kitchen_pose.pose.position.x = -2\n",
"kitchen_pose.pose.position.y = 2\n",
"kitchen_pose.pose.orientation = Quaternion(*quaternion_from_axis_angle([0, 0, 1], -np.pi / 2))\n",
"giskard.world.add_urdf(name='dlr_kitchen',\n",
" urdf=rospy.get_param('kitchen_description'),\n",
" pose=kitchen_pose)\n"
],
"id": "f4a04a641df84a39"
},
{
"metadata": {},
"cell_type": "code",
"outputs": [],
"execution_count": null,
"source": [
"default_pose = {\n",
" \"torso1_joint\": 0.0,\n",
" \"torso2_joint\": -0.9,\n",
" \"torso3_joint\": 1.26,\n",
" \"head1_joint\": 0.0,\n",
" \"head2_joint\": 0.0\n",
"}\n",
"default_left_arm = {\n",
" \"left_arm1_joint\": 0.41,\n",
" \"left_arm2_joint\": -1.64,\n",
" \"left_arm3_joint\": 0.12,\n",
" \"left_arm4_joint\": 0.96,\n",
" \"left_arm5_joint\": 0.71,\n",
" \"left_arm6_joint\": -0.02,\n",
" \"left_arm7_joint\": 0.43\n",
"}\n",
"default_right_arm = {\n",
" \"right_arm1_joint\": 0.6,\n",
" \"right_arm2_joint\": -1.59,\n",
" \"right_arm3_joint\": 2.97,\n",
" \"right_arm4_joint\": -0.99,\n",
" \"right_arm5_joint\": -2.44,\n",
" \"right_arm6_joint\": 0.0,\n",
" \"right_arm7_joint\": 0.0,\n",
"}\n",
"\n",
"better_pose = default_pose\n",
"better_pose.update(default_left_arm)\n",
"better_pose.update(default_right_arm)\n",
"right_closed = {\n",
" \"right_1thumb1_joint\": 0.0,\n",
" \"right_1thumb2_joint\": 0.5,\n",
" \"right_1thumb3_joint\": 0.5,\n",
" \"right_3middle1_joint\": 0.0,\n",
" \"right_3middle2_joint\": 0.5,\n",
" \"right_3middle3_joint\": 0.5,\n",
" \"right_4ring1_joint\": 0.0,\n",
" \"right_4ring2_joint\": 0.5,\n",
" \"right_4ring3_joint\": 0.5,\n",
" \"right_2tip1_joint\": 0.0,\n",
" \"right_2tip2_joint\": 0.5,\n",
" \"right_2tip3_joint\": 0.5,\n",
"}\n",
"left_open = {\n",
" \"left_1thumb1_joint\": 0.0,\n",
" \"left_1thumb2_joint\": 0.0,\n",
" \"left_1thumb3_joint\": 0.0,\n",
" \"left_3middle1_joint\": 0.0,\n",
" \"left_3middle2_joint\": 0.0,\n",
" \"left_3middle3_joint\": 0.0,\n",
" \"left_4ring1_joint\": 0.0,\n",
" \"left_4ring2_joint\": 0.0,\n",
" \"left_4ring3_joint\": 0.0,\n",
" \"left_2tip1_joint\": 0.0,\n",
" \"left_2tip2_joint\": 0.0,\n",
" \"left_2tip3_joint\": 0.0,\n",
"}\n",
"left_closed = {\n",
" \"left_1thumb1_joint\": 0.0,\n",
" \"left_1thumb2_joint\": 0.5,\n",
" \"left_1thumb3_joint\": 0.5,\n",
" \"left_3middle1_joint\": 0.0,\n",
" \"left_3middle2_joint\": 0.5,\n",
" \"left_3middle3_joint\": 0.5,\n",
" \"left_4ring1_joint\": 0.0,\n",
" \"left_4ring2_joint\": 0.5,\n",
" \"left_4ring3_joint\": 0.5,\n",
" \"left_2tip1_joint\": 0.0,\n",
" \"left_2tip2_joint\": 0.5,\n",
" \"left_2tip3_joint\": 0.5,\n",
"}\n",
"right_open = {\n",
" \"right_1thumb1_joint\": 0.0,\n",
" \"right_1thumb2_joint\": 0.0,\n",
" \"right_1thumb3_joint\": 0.0,\n",
" \"right_3middle1_joint\": 0.0,\n",
" \"right_3middle2_joint\": 0.0,\n",
" \"right_3middle3_joint\": 0.0,\n",
" \"right_4ring1_joint\": 0.0,\n",
" \"right_4ring2_joint\": 0.0,\n",
" \"right_4ring3_joint\": 0.0,\n",
" \"right_2tip1_joint\": 0.0,\n",
" \"right_2tip2_joint\": 0.0,\n",
" \"right_2tip3_joint\": 0.0,\n",
"}"
],
"id": "fe3196e049c7aebf"
},
{
"metadata": {},
"cell_type": "code",
"execution_count": null,
"source": [
"giskard.motion_goals.add_joint_position(better_pose)\n",
"giskard.add_default_end_motion_conditions()\n",
"giskard.execute()\n",
"\n",
"handle_frame_id = 'dlr_kitchen/fridge_door_handle'\n",
"handle_name = 'fridge_door_handle'\n",
"door_joint = 'fridge_door_joint'\n",
"fridge = 'dlr_kitchen/fridge'\n",
"# base_goal = PoseStamped()\n",
"box_name = 'box'\n",
"kitchenette = 'dlr_kitchen/kitchenette'\n",
"box_pose = PoseStamped()\n",
"box_pose.header.frame_id = kitchenette\n",
"box_pose.pose.position.z = 0.22\n",
"box_pose.pose.position.x = -0.15\n",
"box_pose.pose.orientation.w = 1.0\n",
"giskard.world.add_box(name=box_name, size=(0.03, 0.15, 0.2), pose=box_pose, parent_link=kitchenette)\n",
"giskard.world.dye_group(group_name=box_name, rgba=(0.0, 0.0, 1.0, 1.0))\n",
"\n",
"pre_grasp_pose = PoseStamped()\n",
"pre_grasp_pose.header.frame_id = box_name\n",
"pre_grasp_pose.pose.orientation = Quaternion(0, 1, 0, 0)\n",
"pre_grasp_pose.pose.position.z = 0.35\n",
"box_pre_grasped = 'pregrasp pose'\n",
"giskard.motion_goals.add_cartesian_pose(name=box_pre_grasped,\n",
" goal_pose=pre_grasp_pose,\n",
" tip_link='r_gripper_tool_frame',\n",
" root_link='map', end_condition=box_pre_grasped)\n",
"\n",
"grasp_pose = deepcopy(pre_grasp_pose)\n",
"grasp_pose.pose.position.z -= 0.2\n",
"box_grasped = giskard.motion_goals.add_cartesian_pose(name='grasp box',\n",
" goal_pose=grasp_pose,\n",
" tip_link='r_gripper_tool_frame',\n",
" root_link='map',\n",
" start_condition=box_pre_grasped)\n",
"right_hand_closed = giskard.motion_goals.add_joint_position(name='close right hand',\n",
" goal_state=right_closed,\n",
" start_condition=box_pre_grasped)\n",
"\n",
"# dlr_kitchen_setup.tasks.add_maximize_manipulability(name='maximize manipulability right',\n",
"# tip_link=dlr_kitchen_setup.r_tip,\n",
"# root_link='torso4')\n",
"\n",
"giskard.monitors.add_end_motion(start_condition=f'{box_grasped} and {right_hand_closed}')\n",
"# dlr_kitchen_setup.motion_goals.allow_self_collision(end_condition=box_pre_grasped)\n",
"giskard.motion_goals.allow_collision(group1='rollin_justin', group2=box_name)\n",
"giskard.motion_goals.allow_self_collision()\n",
"giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')\n",
"giskard.execute()\n",
"\n",
"# %%\n",
"giskard.world.update_parent_link_of_group(name=box_name, parent_link='r_gripper_tool_frame')\n",
"\n",
"# %% HAND OVER\n",
"base_pose = PoseStamped()\n",
"base_pose.header.frame_id = 'base_footprint'\n",
"base_pose.pose.orientation.w = 1.0\n",
"base_pose.pose.position.x = -1\n",
"drove_back = giskard.motion_goals.add_cartesian_pose(name='drive back',\n",
" goal_pose=base_pose,\n",
" tip_link='base_footprint',\n",
" root_link='map')\n",
"\n",
"hand_over_pose = PoseStamped()\n",
"hand_over_pose.header.frame_id = 'l_gripper_tool_frame'\n",
"hand_over_pose.pose.orientation = Quaternion(1, 0, 0, 0)\n",
"hand_over_pose.pose.position.z = 0.3\n",
"handed_over = 'hand over'\n",
"giskard.motion_goals.add_cartesian_pose(name=handed_over,\n",
" goal_pose=hand_over_pose,\n",
" tip_link='r_gripper_tool_frame',\n",
" root_link='l_gripper_tool_frame',\n",
" end_condition=handed_over)\n",
"left_hand_opened = 'open left hand'\n",
"giskard.motion_goals.add_joint_position(name=left_hand_opened,\n",
" goal_state=left_open,\n",
" end_condition=left_hand_opened)\n",
"left_hand_closed = 'close left hand'\n",
"giskard.motion_goals.add_joint_position(name=left_hand_closed,\n",
" goal_state=left_closed,\n",
" start_condition=f'{handed_over} and {left_hand_opened}',\n",
" end_condition=left_hand_closed)\n",
"right_hand_opened = 'open right hand'\n",
"giskard.motion_goals.add_joint_position(name=right_hand_opened,\n",
" goal_state=right_open,\n",
" start_condition=f'{left_hand_closed}',\n",
" end_condition=right_hand_opened)\n",
"# dlr_kitchen_setup.tasks.add_maximize_manipulability(name='maximize manipulability right',\n",
"# tip_link=dlr_kitchen_setup.r_tip,\n",
"# root_link='torso4')\n",
"# dlr_kitchen_setup.tasks.add_maximize_manipulability(name='maximize manipulability left',\n",
"# tip_link=dlr_kitchen_setup.l_tip,\n",
"# root_link='torso4')\n",
"\n",
"giskard.monitors.add_end_motion(start_condition=f'{right_hand_opened} and {drove_back}')\n",
"giskard.motion_goals.allow_collision(group1='rollin_justin', group2=box_name)\n",
"giskard.motion_goals.allow_self_collision()\n",
"giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')\n",
"giskard.execute()\n",
"\n",
"# %%\n",
"giskard.world.update_parent_link_of_group(name=box_name, parent_link='l_gripper_tool_frame')\n",
"# %% PLACE\n",
"in_default_pose = 'default joint pose'\n",
"giskard.motion_goals.add_joint_position(name=in_default_pose,\n",
" goal_state=better_pose,\n",
" end_condition=in_default_pose)\n",
"handle_grasp_pose = PoseStamped()\n",
"handle_grasp_pose.header.frame_id = handle_frame_id\n",
"handle_grasp_pose.pose.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)\n",
"handle_grasp_pose.pose.position.x = -0.12\n",
"handle_graped = 'grasp handle'\n",
"giskard.motion_goals.add_cartesian_pose(name=handle_graped,\n",
" goal_pose=handle_grasp_pose,\n",
" tip_link='r_gripper_tool_frame',\n",
" root_link='map',\n",
" start_condition=in_default_pose,\n",
" end_condition=handle_graped)\n",
"right_hand_closed = 'close right hand'\n",
"giskard.motion_goals.add_joint_position(name=right_hand_closed,\n",
" goal_state=right_closed,\n",
" start_condition=f'{handle_graped}',\n",
" end_condition=right_hand_closed)\n",
"\n",
"door_open = 'open fridge'\n",
"giskard.motion_goals.add_open_container(name=door_open,\n",
" tip_link='r_gripper_tool_frame',\n",
" environment_link=handle_name,\n",
" start_condition=right_hand_closed,\n",
" end_condition='')\n",
"door_half_open = 'is door half open?'\n",
"giskard.monitors.add_joint_position(name=door_half_open,\n",
" goal_state={door_joint: np.pi / 4},\n",
" start_condition=handle_graped,\n",
" end_condition=door_half_open)\n",
"\n",
"place_pose = PoseStamped()\n",
"place_pose.header.frame_id = fridge\n",
"place_pose.pose.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)\n",
"place_pose.pose.position.z = 0.4\n",
"place_pose.pose.position.x = 0.\n",
"box_placed = 'place box'\n",
"giskard.motion_goals.add_cartesian_pose(name=box_placed,\n",
" goal_pose=place_pose,\n",
" tip_link=box_name,\n",
" root_link='map',\n",
" start_condition=door_half_open,\n",
" end_condition=box_placed)\n",
"left_hand_opened = 'open left hand'\n",
"giskard.motion_goals.add_joint_position(name=left_hand_opened,\n",
" goal_state=left_open,\n",
" start_condition=f'{box_placed}',\n",
" end_condition=left_hand_opened)\n",
"\n",
"done = f'{door_open} and {left_hand_opened}'\n",
"giskard.monitors.add_end_motion(start_condition=done)\n",
"giskard.motion_goals.allow_collision(group2='right_hand',\n",
" group1='dlr_kitchen')\n",
"giskard.motion_goals.allow_self_collision()\n",
"giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')\n",
"giskard.execute()\n",
"\n",
"# %%\n",
"giskard.world.update_parent_link_of_group(name=box_name, parent_link=fridge)\n",
"\n",
"# %%\n",
"retract_pose = PoseStamped()\n",
"retract_pose.header.frame_id = 'l_gripper_tool_frame'\n",
"retract_pose.pose.orientation.w = 1.0\n",
"retract_pose.pose.position.z = -0.5\n",
"retracted = 'retract left hand'\n",
"giskard.motion_goals.add_joint_position(name=retracted,\n",
" goal_state=default_left_arm,\n",
" end_condition=retracted)\n",
"delay = giskard.monitors.add_sleep(name='wait 1s', seconds=1)\n",
"giskard.motion_goals.add_open_container(name='hold handle',\n",
" tip_link='r_gripper_tool_frame',\n",
" environment_link=handle_name,\n",
" end_condition=delay)\n",
"\n",
"door_closed = giskard.motion_goals.add_close_container(name='close fridge',\n",
" tip_link='r_gripper_tool_frame',\n",
" environment_link=handle_name,\n",
" start_condition=delay)\n",
"giskard.monitors.add_end_motion(start_condition=f'{door_closed} and {retracted}')\n",
"giskard.motion_goals.allow_collision(group2='right_hand',\n",
" group1='dlr_kitchen')\n",
"giskard.motion_goals.allow_self_collision()\n",
"giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')\n",
"giskard.execute()"
],
"id": "8db5b0dbba7b5fc5",
"outputs": []
}
],
"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": 5
}