robosim.physics.motion

motion.py - Implements the motion dynamics. We support from simple 'level 1' (velocity only) to higher level ones. All the functions here receive the current Physics state and motion input (level-dependent) and return velocities (v, w)

 1"""
 2motion.py - Implements the motion dynamics. We support from simple 'level 1' (velocity only) to higher level ones.
 3All the functions here receive the current Physics state and motion input (level-dependent) and return velocities (v, w)
 4"""
 5import random
 6import numpy as np
 7from microecs import Field
 8from robosim.utils import renormalize_rotation_matrix, btrexp, bskewa
 9from robosim.constants import EPS
10
11Arr = np.ndarray | Field
12
13def integrate_velocity_into_pose(pose: Arr, velocity: Arr, dt: float) -> np.ndarray:
14    """integrates pose[t+1] = pose[t] + v[t] * dt with v=(v, w) twist. Uses rodrigues formula (batched)."""
15    pose = pose.numpy() if isinstance(pose, Field) else pose                 # (N, 4, 4)
16    velocity = velocity.numpy() if isinstance(velocity, Field) else velocity # (N, 6) [m/s]
17    res = np.float32(pose) @ btrexp(bskewa(velocity * dt)).astype("float32") # (N, 4, 4) [m]
18    if random.random() >= 0.99: # renormalize every now and then to ensure rotation matrix is still reliable
19        for i in range(len(pose)):
20            res[i, 0:3, 0:3] = renormalize_rotation_matrix(res[i, 0:3, 0:3])
21    return res
22
23def motion_level1(motion_input: Arr, max_velocities: Arr) -> Arr:
24    """level 1 physics: velocity is the motion input. Returns it after clamping"""
25    velocity = np.clip(motion_input, -1, 1) * max_velocities              # [m/s]
26    return velocity
27
28def motion_level2(motion_input: Arr, dt: float, current_velocity: Arr,
29                  max_accelerations: Arr, drag_coefficient: Arr) -> tuple[Arr, Arr]:
30    """
31    level 2 physics: integrates acceleration (motion input) + drag (ct) into the current velocity (A = (a[t] - k * v[t])
32    - v[t+1] = v[t] + dt * (a[t] - k * v[t])
33    """
34    clamped_acceleration = np.clip(motion_input, -1, 1) * max_accelerations
35    drag = current_velocity * drag_coefficient                            # [m/s**2]; (N, 6) * (N, )
36    acceleration = clamped_acceleration - drag                            # [m/s**2]; (N, 6) - (N, 6)
37    velocity = current_velocity + dt * acceleration                       # [m/s]
38    # clamp off super small velocities so they don't go "forever" in the tests
39    velocity = np.where(np.abs(velocity) < EPS, 0, velocity)              # [m/s]
40    return velocity, acceleration
Arr = numpy.ndarray | microecs.query_result.Field
def integrate_velocity_into_pose( pose: numpy.ndarray | microecs.query_result.Field, velocity: numpy.ndarray | microecs.query_result.Field, dt: float) -> numpy.ndarray: 
14def integrate_velocity_into_pose(pose: Arr, velocity: Arr, dt: float) -> np.ndarray:
15    """integrates pose[t+1] = pose[t] + v[t] * dt with v=(v, w) twist. Uses rodrigues formula (batched)."""
16    pose = pose.numpy() if isinstance(pose, Field) else pose                 # (N, 4, 4)
17    velocity = velocity.numpy() if isinstance(velocity, Field) else velocity # (N, 6) [m/s]
18    res = np.float32(pose) @ btrexp(bskewa(velocity * dt)).astype("float32") # (N, 4, 4) [m]
19    if random.random() >= 0.99: # renormalize every now and then to ensure rotation matrix is still reliable
20        for i in range(len(pose)):
21            res[i, 0:3, 0:3] = renormalize_rotation_matrix(res[i, 0:3, 0:3])
22    return res

integrates pose[t+1] = pose[t] + v[t] * dt with v=(v, w) twist. Uses rodrigues formula (batched).

def motion_level1( motion_input: numpy.ndarray | microecs.query_result.Field, max_velocities: numpy.ndarray | microecs.query_result.Field) -> numpy.ndarray | microecs.query_result.Field: 
24def motion_level1(motion_input: Arr, max_velocities: Arr) -> Arr:
25    """level 1 physics: velocity is the motion input. Returns it after clamping"""
26    velocity = np.clip(motion_input, -1, 1) * max_velocities              # [m/s]
27    return velocity

level 1 physics: velocity is the motion input. Returns it after clamping

def motion_level2( motion_input: numpy.ndarray | microecs.query_result.Field, dt: float, current_velocity: numpy.ndarray | microecs.query_result.Field, max_accelerations: numpy.ndarray | microecs.query_result.Field, drag_coefficient: numpy.ndarray | microecs.query_result.Field) -> tuple[numpy.ndarray | microecs.query_result.Field, numpy.ndarray | microecs.query_result.Field]: 
29def motion_level2(motion_input: Arr, dt: float, current_velocity: Arr,
30                  max_accelerations: Arr, drag_coefficient: Arr) -> tuple[Arr, Arr]:
31    """
32    level 2 physics: integrates acceleration (motion input) + drag (ct) into the current velocity (A = (a[t] - k * v[t])
33    - v[t+1] = v[t] + dt * (a[t] - k * v[t])
34    """
35    clamped_acceleration = np.clip(motion_input, -1, 1) * max_accelerations
36    drag = current_velocity * drag_coefficient                            # [m/s**2]; (N, 6) * (N, )
37    acceleration = clamped_acceleration - drag                            # [m/s**2]; (N, 6) - (N, 6)
38    velocity = current_velocity + dt * acceleration                       # [m/s]
39    # clamp off super small velocities so they don't go "forever" in the tests
40    velocity = np.where(np.abs(velocity) < EPS, 0, velocity)              # [m/s]
41    return velocity, acceleration

level 2 physics: integrates acceleration (motion input) + drag (ct) into the current velocity (A = (a[t] - k * v[t])

  • v[t+1] = v[t] + dt * (a[t] - k * v[t])