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")