-
Notifications
You must be signed in to change notification settings - Fork 29
/
test_integration_boxy.py
144 lines (117 loc) · 5.7 KB
/
test_integration_boxy.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
from copy import copy
import numpy as np
import pytest
from geometry_msgs.msg import PoseStamped, Quaternion, Vector3Stamped, PointStamped
from tf.transformations import quaternion_from_matrix
from giskardpy.utils.tfwrapper import lookup_point, transform_point, \
transform_pose
from utils_for_tests import Donbot, Boxy
# TODO roslaunch iai_boxy_sim ros_control_sim.launch
@pytest.fixture(scope='module')
def giskard(request, ros):
c = Boxy()
request.addfinalizer(c.tear_down)
return c
class TestJointGoals(object):
def test_joint_movement1(self, zero_pose):
"""
:type zero_pose: Donbot
"""
zero_pose.allow_self_collision()
js = copy(zero_pose.better_pose)
js['triangle_base_joint'] = zero_pose.default_pose['triangle_base_joint']
zero_pose.set_joint_goal(js)
zero_pose.plan_and_execute()
class TestConstraints(object):
def test_pointing(self, better_pose):
"""
:type better_pose: Boxy
"""
tip = 'head_mount_kinect2_rgb_optical_frame'
goal_point = lookup_point('map', better_pose.r_tip)
better_pose.set_pointing_goal(tip, goal_point)
better_pose.plan_and_execute()
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = transform_point(tip, goal_point)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
goal_point = lookup_point('map', better_pose.r_tip)
better_pose.set_pointing_goal(tip, goal_point, root_link=better_pose.r_tip)
r_goal = PoseStamped()
r_goal.header.frame_id = better_pose.r_tip
r_goal.pose.position.x -= 0.2
r_goal.pose.position.z -= 0.5
r_goal.pose.orientation.w = 1
r_goal = transform_pose(better_pose.default_root, r_goal)
r_goal.pose.orientation = Quaternion(*quaternion_from_matrix([[0, 0, 1, 0],
[0, -1, 0, 0],
[1, 0, 0, 0],
[0, 0, 0, 1]]))
better_pose.set_cart_goal(r_goal, better_pose.r_tip, 'base_footprint')
better_pose.plan_and_execute()
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = lookup_point(tip, better_pose.r_tip)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
def test_open_drawer(self, kitchen_setup): # where is the kitchen_setup actually loaded
""""
:type kitchen_setup: Boxy
"""
handle_frame_id = 'iai_kitchen/sink_area_left_middle_drawer_handle'
handle_name = 'sink_area_left_middle_drawer_handle'
bar_axis = Vector3Stamped()
bar_axis.header.frame_id = handle_frame_id
bar_axis.vector.y = 1
bar_center = PointStamped()
bar_center.header.frame_id = handle_frame_id
tip_grasp_axis = Vector3Stamped()
tip_grasp_axis.header.frame_id = kitchen_setup.l_tip
tip_grasp_axis.vector.y = 1
kitchen_setup.set_json_goal('GraspBar',
root_link=kitchen_setup.default_root,
tip_link=kitchen_setup.l_tip,
tip_grasp_axis=tip_grasp_axis,
bar_center=bar_center,
bar_axis=bar_axis,
bar_length=0.4)
# Create gripper from kitchen object
x_gripper = Vector3Stamped()
x_gripper.header.frame_id = kitchen_setup.l_tip
x_gripper.vector.z = 1
# Get goal for grasping the handle
x_goal = Vector3Stamped()
x_goal.header.frame_id = handle_frame_id
x_goal.vector.x = -1
# Align planes for gripper to be horizontal/vertical
kitchen_setup.set_align_planes_goal(kitchen_setup.l_tip,
x_gripper,
root_normal=x_goal)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.plan_and_execute() # send goal to Giskard
kitchen_setup.set_json_goal('Open',
tip_link=kitchen_setup.l_tip,
environment_link=handle_name)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.plan_and_execute() # send goal to Giskard
# Update kitchen object
kitchen_setup.set_kitchen_js({'sink_area_left_middle_drawer_main_joint': 0.48})
# Close drawer partially
kitchen_setup.set_json_goal('Open',
tip_link=kitchen_setup.l_tip,
environment_link=handle_name,
goal_joint_state=0.2)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.plan_and_execute() # send goal to Giskard
# Update kitchen object
kitchen_setup.set_kitchen_js({'sink_area_left_middle_drawer_main_joint': 0.2})
kitchen_setup.set_json_goal('Close',
tip_link=kitchen_setup.l_tip,
environment_link=handle_name)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.plan_and_execute() # send goal to Giskard
# Update kitchen object
kitchen_setup.set_kitchen_js({'sink_area_left_middle_drawer_main_joint': 0.0})