def go_to_kitchen():
kitchen_pose = geometry.Pose(pos=geometry.Vector3(x=3.11, y=1.21, z=0.0),
ori=geometry.Quaternion(
x=0.0,
y=0.0,
z=-0.01023836327822357,
w=0.9999475865851083))
omni_base.go_pose(kitchen_pose)
def go_to_microwave(self):
microwave_pose = geometry.Pose(pos=geometry.Vector3(x=1.909,
y=1.779478,
z=0.0),
ori=geometry.Quaternion(x=0.0,
y=0.0,
z=0.70096336,
w=0.713197))
self.omni_base.go_pose(microwave_pose)
def go_to_yakan():
yakan_pose = geometry.Pose(pos=geometry.Vector3(x=3.108781824048564,
y=0.037752328711472494,
z=0.0),
ori=geometry.Quaternion(x=0.0,
y=0.0,
z=0.013848295774654074,
w=0.9999041077544075))
omni_base.go_pose(yakan_pose)
def go_to_stop_button(self):
stop_button_pose = geometry.Pose(
pos=geometry.Vector3(x=3.1237696626280242,
y=-0.31407748223644616,
z=0.0),
ori=geometry.Quaternion(x=0.0,
y=0.0,
z=-0.01521901673895873,
w=0.9998841840580838))
self.omni_base.go_pose(stop_button_pose)
def go_to_fridge(self):
fridge_pose = geometry.Pose(pos=geometry.Vector3(x=0.1457817782465553,
y=1.5697370723246589,
z=0.0),
ori=geometry.Quaternion(
x=0.0,
y=0.0,
z=0.7400625754780703,
w=0.6725380170494195))
self.omni_base.go_pose(fridge_pose)
def go(self, x, y, yaw, timeout=0.0, relative=False, angular_thresh=10.0, spatial_thresh=0.1):
mobile_base._validate_timeout(timeout)
position = geometry.Vector3(x, y, 0)
quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
orientation = geometry.Quaternion(*quat)
pose = (position, orientation)
if relative:
ref_frame_id = settings.get_frame('base')
else:
ref_frame_id = settings.get_frame('map')
self.move(pose, timeout, ref_frame_id, angular_thresh)
def _pose_from_x_axis(axis):
"""Compute a transformation that fits X-axis of its frame to given vector.
Args:
axis (geometry.Vector3): A target vector
Returns:
geometry.Pose: The result transformation that stored in Pose type.
"""
axis = np.array(axis, dtype='float64', copy=True)
axis = _normalize_np(axis)
unit_x = np.array([1, 0, 0])
outerp = np.cross(unit_x, axis)
theta = math.acos(np.dot(unit_x, axis))
if np.linalg.norm(outerp) < sys.float_info.epsilon:
outerp = np.array([0, 1, 0])
outerp = _normalize_np(outerp)
q = T.quaternion_about_axis(theta, outerp)
return geometry.Pose(geometry.Vector3(0, 0, 0), geometry.Quaternion(*q))
def _plan_robot_action(self,
action,
odom_to_robot_pose,
initial_joint_state,
collision_env=None):
if action.type == Action.POSE:
x = action.odom_to_hand_pose.position.x
y = action.odom_to_hand_pose.position.y
z = action.odom_to_hand_pose.position.z
vec3 = geometry.Vector3(*(x, y, z))
x = action.odom_to_hand_pose.orientation.x
y = action.odom_to_hand_pose.orientation.y
z = action.odom_to_hand_pose.orientation.z
w = action.odom_to_hand_pose.orientation.w
quaternion = geometry.Quaternion(*(x, y, z, w))
pose = geometry.Pose(vec3, quaternion)
return self._move_end_effector_pose(pose, odom_to_robot_pose,
initial_joint_state,
collision_env)
elif action.type == Action.LINE:
x = action.axis.x
y = action.axis.y
z = action.axis.z
axis = geometry.Vector3(*(x, y, z))
return self._move_end_effector_by_line(axis, action.distance,
odom_to_robot_pose,
initial_joint_state,
collision_env)
elif action.type == Action.GRIPPER:
return self._command_gripper(action.open_angle, action.motion_time)
else:
raise Exception("type must be pose, line, or gripper")