예제 #1
0
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)
예제 #2
0
 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)
예제 #3
0
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)
예제 #4
0
 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)
예제 #5
0
 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))
예제 #8
0
    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")