Skip to content

Robots

BaseRobot 

BaseRobot(hertz, state)

Bases: ABC

Base class for a robot.

Initializes the robot with a control rate and initial state.

Parameters:

Name Type Description Default
hertz float

Control rate of the robot [#cycles / second].

 required
state ndarray

Initial state of the robot. Shape: (num_states, ).

 required
Source code in pypolo/robots/base_robot.py 
 9
10
11
12
13
14
15
16
17
18
19
20
def __init__(self, hertz: float, state: np.ndarray):
    r"""Initializes the robot with a control rate and initial state.

    Args:
        hertz (float): Control rate of the robot [#cycles / second].
        state (np.ndarray): Initial state of the robot.
            Shape: (num_states, ).

    """
    self.state = state
    self.hertz = hertz
    self.control_dt = 1.0 / hertz

error abstractmethod 

error(state, goal)

Error function for the robot.

Parameters:

Name Type Description Default
state ndarray

Current state of the robot. Shape: (num_states, ).

 required
goal ndarray

Goal state of the robot. Shape: (num_states, ).

 required

Returns:

Type Description
ndarray

np.ndarray: An error array of shape (num_actions, ).
Source code in pypolo/robots/base_robot.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
57
@abstractmethod
def error(self, state: np.ndarray, goal: np.ndarray) -> np.ndarray:
    r"""Error function for the robot.

    Args:
        state (np.ndarray): Current state of the robot.
            Shape: (num_states, ).
        goal (np.ndarray): Goal state of the robot.
            Shape: (num_states, ).

    Returns:
        np.ndarray: An error array of shape (num_actions, ).
    """
    raise NotImplementedError

step abstractmethod 

step(state, action)

Transition function for the robot.

Parameters:

Name Type Description Default
action ndarray

Action to be taken by the robot.

 required

Returns:

Type Description
ndarray

np.ndarray: New state of the robot.
Source code in pypolo/robots/base_robot.py 
31
32
33
34
35
36
37
38
39
40
41
42
@abstractmethod
def step(self, state: np.ndarray, action: np.ndarray) -> np.ndarray:
    r"""Transition function for the robot.

    Args:
        action (np.ndarray): Action to be taken by the robot.

    Returns:
        np.ndarray: New state of the robot.

    """
    raise NotImplementedError

take 

take(action)

Takes an action and updates the state of the robot.

Parameters:

Name Type Description Default
action ndarray

Action to be taken by the robot.

 required
Source code in pypolo/robots/base_robot.py 
22
23
24
25
26
27
28
29
def take(self, action: np.ndarray) -> None:
    r"""Takes an action and updates the state of the robot.

    Args:
        action (np.ndarray): Action to be taken by the robot.

    """
    self.state = self.step(self.state, action)

DifferentialDriveRobot 

DifferentialDriveRobot(hertz, state)

Bases: BaseRobot

A robot with differential drive wheels.

Initializes the robot with a control rate and initial state.

Parameters:

Name Type Description Default
hertz float

Control rate of the robot [#cycles / second].

 required
state ndarray

Initial state of the robot [x, y, θ, v, ω]. Shape: (num_states, ).

 required
Source code in pypolo/robots/differential_drive_robot.py 
10
11
12
13
14
15
16
17
18
19
20
21
22
23
def __init__(self, hertz: float, state: np.ndarray):
    r"""Initializes the robot with a control rate and initial state.

    Args:
        hertz (float): Control rate of the robot [#cycles / second].
        state (np.ndarray): Initial state of the robot [x, y, θ, v, ω].
            Shape: (num_states, ).

    """
    assert hertz > 0.0
    assert state.shape == (5, )
    self.state = state
    self.hertz = hertz
    self.control_dt = 1.0 / hertz

error 

error(x, y)

Given a goal location [x, y], returns an error array with the same shape as the action space.

Parameters:

Name Type Description Default
x float

Goal x position.

 required
y float

Goal y position.

 required

Returns:

Type Description
ndarray

np.ndarray: An error array of shape (num_actions, ).
Source code in pypolo/robots/differential_drive_robot.py 
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
def error(self, x: float, y: float) -> np.ndarray:
    r"""Given a goal location [x, y], returns an error array with the same
    shape as the action space.

    Args:
        x (float): Goal x position.
        y (float): Goal y position.

    Returns:
        np.ndarray: An error array of shape (num_actions, ).

    """
    error_x = x - self.state[0]
    error_y = y - self.state[1]
    linear_error = math.sqrt(error_x**2 + error_y**2)
    error_omega = math.atan2(error_y, error_x) - self.state[2]
    angular_error = (error_omega + math.pi) % (2 * math.pi) - math.pi
    return np.array([linear_error, angular_error])

step 

step(state, action)

Transition function for the robot.

Parameters:

Name Type Description Default
state ndarray

Current state of the robot [x, y, θ, v, ω]].

 required
action ndarray

Action to be taken by the robot [dv, dω].

 required

Returns:

Type Description
ndarray

np.ndarray: New state of the robot.
Source code in pypolo/robots/differential_drive_robot.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
def step(self, state: np.ndarray, action: np.ndarray) -> np.ndarray:
    r"""Transition function for the robot.

    Args:
        state (np.ndarray): Current state of the robot [x, y, θ, v, ω]].
        action (np.ndarray): Action to be taken by the robot [dv, dω].

    Returns:
        np.ndarray: New state of the robot.

    """
    assert state.shape == (5, )
    assert action.shape == (2, )
    x, y, theta, v, omega = state
    delta_v, delta_omega = action
    delta_x = v * math.cos(theta) * self.control_dt
    delta_y = v * math.sin(theta) * self.control_dt
    delta_theta = omega * self.control_dt
    new_x = x + delta_x
    new_y = y + delta_y
    new_theta = (theta + delta_theta) % (2 * math.pi)
    new_v = v + delta_v
    new_omega = omega + delta_omega
    return np.array([new_x, new_y, new_theta, new_v, new_omega])