%%bash
cd /home/jovyan/workspace/ros/src/giskardpy_ros/ && git checkout c4a25152faa05246a4c263b22b76696b7bc6a72a

%%bash --bg
roslaunch giskardpy_ros giskardpy_justin_standalone.launch

import rospy
from geometry_msgs.msg import PoseStamped, Quaternion
import numpy as np
from giskardpy.utils.math import quaternion_from_axis_angle
from giskardpy_ros.python_interface.python_interface import GiskardWrapper
from copy import deepcopy

rospy.init_node('justin_demo')

giskard = GiskardWrapper()

kitchen_pose = PoseStamped()
kitchen_pose.header.frame_id = 'map'
kitchen_pose.pose.position.x = -2
kitchen_pose.pose.position.y = 2
kitchen_pose.pose.orientation = Quaternion(*quaternion_from_axis_angle([0, 0, 1], -np.pi / 2))
giskard.world.add_urdf(name='dlr_kitchen',
                          urdf=rospy.get_param('kitchen_description'),
                          pose=kitchen_pose)

default_pose = {
    "torso1_joint": 0.0,
    "torso2_joint": -0.9,
    "torso3_joint": 1.26,
    "head1_joint": 0.0,
    "head2_joint": 0.0
}
default_left_arm = {
    "left_arm1_joint": 0.41,
    "left_arm2_joint": -1.64,
    "left_arm3_joint": 0.12,
    "left_arm4_joint": 0.96,
    "left_arm5_joint": 0.71,
    "left_arm6_joint": -0.02,
    "left_arm7_joint": 0.43
}
default_right_arm = {
    "right_arm1_joint": 0.6,
    "right_arm2_joint": -1.59,
    "right_arm3_joint": 2.97,
    "right_arm4_joint": -0.99,
    "right_arm5_joint": -2.44,
    "right_arm6_joint": 0.0,
    "right_arm7_joint": 0.0,
}

better_pose = default_pose
better_pose.update(default_left_arm)
better_pose.update(default_right_arm)
right_closed = {
    "right_1thumb1_joint": 0.0,
    "right_1thumb2_joint": 0.5,
    "right_1thumb3_joint": 0.5,
    "right_3middle1_joint": 0.0,
    "right_3middle2_joint": 0.5,
    "right_3middle3_joint": 0.5,
    "right_4ring1_joint": 0.0,
    "right_4ring2_joint": 0.5,
    "right_4ring3_joint": 0.5,
    "right_2tip1_joint": 0.0,
    "right_2tip2_joint": 0.5,
    "right_2tip3_joint": 0.5,
}
left_open = {
    "left_1thumb1_joint": 0.0,
    "left_1thumb2_joint": 0.0,
    "left_1thumb3_joint": 0.0,
    "left_3middle1_joint": 0.0,
    "left_3middle2_joint": 0.0,
    "left_3middle3_joint": 0.0,
    "left_4ring1_joint": 0.0,
    "left_4ring2_joint": 0.0,
    "left_4ring3_joint": 0.0,
    "left_2tip1_joint": 0.0,
    "left_2tip2_joint": 0.0,
    "left_2tip3_joint": 0.0,
}
left_closed = {
    "left_1thumb1_joint": 0.0,
    "left_1thumb2_joint": 0.5,
    "left_1thumb3_joint": 0.5,
    "left_3middle1_joint": 0.0,
    "left_3middle2_joint": 0.5,
    "left_3middle3_joint": 0.5,
    "left_4ring1_joint": 0.0,
    "left_4ring2_joint": 0.5,
    "left_4ring3_joint": 0.5,
    "left_2tip1_joint": 0.0,
    "left_2tip2_joint": 0.5,
    "left_2tip3_joint": 0.5,
}
right_open = {
    "right_1thumb1_joint": 0.0,
    "right_1thumb2_joint": 0.0,
    "right_1thumb3_joint": 0.0,
    "right_3middle1_joint": 0.0,
    "right_3middle2_joint": 0.0,
    "right_3middle3_joint": 0.0,
    "right_4ring1_joint": 0.0,
    "right_4ring2_joint": 0.0,
    "right_4ring3_joint": 0.0,
    "right_2tip1_joint": 0.0,
    "right_2tip2_joint": 0.0,
    "right_2tip3_joint": 0.0,
}

giskard.motion_goals.add_joint_position(better_pose)
giskard.add_default_end_motion_conditions()
giskard.execute()

handle_frame_id = 'dlr_kitchen/fridge_door_handle'
handle_name = 'fridge_door_handle'
door_joint = 'fridge_door_joint'
fridge = 'dlr_kitchen/fridge'
# base_goal = PoseStamped()
box_name = 'box'
kitchenette = 'dlr_kitchen/kitchenette'
box_pose = PoseStamped()
box_pose.header.frame_id = kitchenette
box_pose.pose.position.z = 0.22
box_pose.pose.position.x = -0.15
box_pose.pose.orientation.w = 1.0
giskard.world.add_box(name=box_name, size=(0.03, 0.15, 0.2), pose=box_pose, parent_link=kitchenette)
giskard.world.dye_group(group_name=box_name, rgba=(0.0, 0.0, 1.0, 1.0))

pre_grasp_pose = PoseStamped()
pre_grasp_pose.header.frame_id = box_name
pre_grasp_pose.pose.orientation = Quaternion(0, 1, 0, 0)
pre_grasp_pose.pose.position.z = 0.35
box_pre_grasped = 'pregrasp pose'
giskard.motion_goals.add_cartesian_pose(name=box_pre_grasped,
                                        goal_pose=pre_grasp_pose,
                                        tip_link='r_gripper_tool_frame',
                                        root_link='map', end_condition=box_pre_grasped)

grasp_pose = deepcopy(pre_grasp_pose)
grasp_pose.pose.position.z -= 0.2
box_grasped = giskard.motion_goals.add_cartesian_pose(name='grasp box',
                                                      goal_pose=grasp_pose,
                                                      tip_link='r_gripper_tool_frame',
                                                      root_link='map',
                                                      start_condition=box_pre_grasped)
right_hand_closed = giskard.motion_goals.add_joint_position(name='close right hand',
                                                            goal_state=right_closed,
                                                            start_condition=box_pre_grasped)

# dlr_kitchen_setup.tasks.add_maximize_manipulability(name='maximize manipulability right',
#                                                     tip_link=dlr_kitchen_setup.r_tip,
#                                                     root_link='torso4')

giskard.monitors.add_end_motion(start_condition=f'{box_grasped} and {right_hand_closed}')
# dlr_kitchen_setup.motion_goals.allow_self_collision(end_condition=box_pre_grasped)
giskard.motion_goals.allow_collision(group1='rollin_justin', group2=box_name)
giskard.motion_goals.allow_self_collision()
giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')
giskard.execute()

# %%
giskard.world.update_parent_link_of_group(name=box_name, parent_link='r_gripper_tool_frame')

# %% HAND OVER
base_pose = PoseStamped()
base_pose.header.frame_id = 'base_footprint'
base_pose.pose.orientation.w = 1.0
base_pose.pose.position.x = -1
drove_back = giskard.motion_goals.add_cartesian_pose(name='drive back',
                                                     goal_pose=base_pose,
                                                     tip_link='base_footprint',
                                                     root_link='map')

hand_over_pose = PoseStamped()
hand_over_pose.header.frame_id = 'l_gripper_tool_frame'
hand_over_pose.pose.orientation = Quaternion(1, 0, 0, 0)
hand_over_pose.pose.position.z = 0.3
handed_over = 'hand over'
giskard.motion_goals.add_cartesian_pose(name=handed_over,
                                        goal_pose=hand_over_pose,
                                        tip_link='r_gripper_tool_frame',
                                        root_link='l_gripper_tool_frame',
                                        end_condition=handed_over)
left_hand_opened = 'open left hand'
giskard.motion_goals.add_joint_position(name=left_hand_opened,
                                        goal_state=left_open,
                                        end_condition=left_hand_opened)
left_hand_closed = 'close left hand'
giskard.motion_goals.add_joint_position(name=left_hand_closed,
                                        goal_state=left_closed,
                                        start_condition=f'{handed_over} and {left_hand_opened}',
                                        end_condition=left_hand_closed)
right_hand_opened = 'open right hand'
giskard.motion_goals.add_joint_position(name=right_hand_opened,
                                        goal_state=right_open,
                                        start_condition=f'{left_hand_closed}',
                                        end_condition=right_hand_opened)
# dlr_kitchen_setup.tasks.add_maximize_manipulability(name='maximize manipulability right',
#                                                     tip_link=dlr_kitchen_setup.r_tip,
#                                                     root_link='torso4')
# dlr_kitchen_setup.tasks.add_maximize_manipulability(name='maximize manipulability left',
#                                                     tip_link=dlr_kitchen_setup.l_tip,
#                                                     root_link='torso4')

giskard.monitors.add_end_motion(start_condition=f'{right_hand_opened} and {drove_back}')
giskard.motion_goals.allow_collision(group1='rollin_justin', group2=box_name)
giskard.motion_goals.allow_self_collision()
giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')
giskard.execute()

# %%
giskard.world.update_parent_link_of_group(name=box_name, parent_link='l_gripper_tool_frame')
# %% PLACE
in_default_pose = 'default joint pose'
giskard.motion_goals.add_joint_position(name=in_default_pose,
                                        goal_state=better_pose,
                                        end_condition=in_default_pose)
handle_grasp_pose = PoseStamped()
handle_grasp_pose.header.frame_id = handle_frame_id
handle_grasp_pose.pose.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)
handle_grasp_pose.pose.position.x = -0.12
handle_graped = 'grasp handle'
giskard.motion_goals.add_cartesian_pose(name=handle_graped,
                                                  goal_pose=handle_grasp_pose,
                                                  tip_link='r_gripper_tool_frame',
                                                  root_link='map',
                                                  start_condition=in_default_pose,
                                                  end_condition=handle_graped)
right_hand_closed = 'close right hand'
giskard.motion_goals.add_joint_position(name=right_hand_closed,
                                                  goal_state=right_closed,
                                                  start_condition=f'{handle_graped}',
                                                  end_condition=right_hand_closed)

door_open = 'open fridge'
giskard.motion_goals.add_open_container(name=door_open,
                                                  tip_link='r_gripper_tool_frame',
                                                  environment_link=handle_name,
                                                  start_condition=right_hand_closed,
                                                  end_condition='')
door_half_open = 'is door half open?'
giskard.monitors.add_joint_position(name=door_half_open,
                                              goal_state={door_joint: np.pi / 4},
                                              start_condition=handle_graped,
                                              end_condition=door_half_open)

place_pose = PoseStamped()
place_pose.header.frame_id = fridge
place_pose.pose.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)
place_pose.pose.position.z = 0.4
place_pose.pose.position.x = 0.
box_placed = 'place box'
giskard.motion_goals.add_cartesian_pose(name=box_placed,
                                                  goal_pose=place_pose,
                                                  tip_link=box_name,
                                                  root_link='map',
                                                  start_condition=door_half_open,
                                                  end_condition=box_placed)
left_hand_opened = 'open left hand'
giskard.motion_goals.add_joint_position(name=left_hand_opened,
                                                  goal_state=left_open,
                                                  start_condition=f'{box_placed}',
                                                  end_condition=left_hand_opened)

done = f'{door_open} and {left_hand_opened}'
giskard.monitors.add_end_motion(start_condition=done)
giskard.motion_goals.allow_collision(group2='right_hand',
                                  group1='dlr_kitchen')
giskard.motion_goals.allow_self_collision()
giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')
giskard.execute()

# %%
giskard.world.update_parent_link_of_group(name=box_name, parent_link=fridge)

# %%
retract_pose = PoseStamped()
retract_pose.header.frame_id = 'l_gripper_tool_frame'
retract_pose.pose.orientation.w = 1.0
retract_pose.pose.position.z = -0.5
retracted = 'retract left hand'
giskard.motion_goals.add_joint_position(name=retracted,
                                                  goal_state=default_left_arm,
                                                  end_condition=retracted)
delay = giskard.monitors.add_sleep(name='wait 1s', seconds=1)
giskard.motion_goals.add_open_container(name='hold handle',
                                                  tip_link='r_gripper_tool_frame',
                                                  environment_link=handle_name,
                                                  end_condition=delay)

door_closed = giskard.motion_goals.add_close_container(name='close fridge',
                                                                 tip_link='r_gripper_tool_frame',
                                                                 environment_link=handle_name,
                                                                 start_condition=delay)
giskard.monitors.add_end_motion(start_condition=f'{door_closed} and {retracted}')
giskard.motion_goals.allow_collision(group2='right_hand',
                                  group1='dlr_kitchen')
giskard.motion_goals.allow_self_collision()
giskard.motion_goals.add_justin_torso_limit(name='torso4_joint', end_condition='')
giskard.execute()