ChenXi/FCP_gym
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init gym

Browse Source
This commit is contained in:
ChenXi
2025-11-19 08:08:22 -05:00
commit eaaa5519bd
256 changed files with 46657 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

242
math_ops/Inverse_Kinematics.py Normal file
View File

@@ -0,0 +1,242 @@
from math import asin, atan, atan2, pi, sqrt
from math_ops.Matrix_3x3 import Matrix_3x3
from math_ops.Math_Ops import Math_Ops as M
import numpy as np
class Inverse_Kinematics():
# leg y deviation, upper leg height, upper leg depth, lower leg length, knee extra angle, max ankle z
NAO_SPECS_PER_ROBOT = ((0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091),
(0.055, 0.13832, 0.005, 0.11832, atan(0.005/0.13832), -0.106),
(0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091),
(0.072954143,0.147868424, 0.005, 0.127868424, atan(0.005/0.147868424), -0.114),
(0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091))
TORSO_HIP_Z = 0.115 # distance in the z-axis, between the torso and each hip (same for all robots)
TORSO_HIP_X = 0.01 # distance in the x-axis, between the torso and each hip (same for all robots) (hip is 0.01m to the back)
def __init__(self, robot) -> None:
self.robot = robot
self.NAO_SPECS = Inverse_Kinematics.NAO_SPECS_PER_ROBOT[robot.type]
def torso_to_hip_transform(self, coords, is_batch=False):
'''
Convert cartesian coordinates that are relative to torso to coordinates that are relative the center of both hip joints
Parameters
----------
coords : array_like
One 3D position or list of 3D positions
is_batch : `bool`
Indicates if coords is a batch of 3D positions
Returns
-------
coord : `list` or ndarray
A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned
'''
if is_batch:
return [c + (Inverse_Kinematics.TORSO_HIP_X, 0, Inverse_Kinematics.TORSO_HIP_Z) for c in coords]
else:
return coords + (Inverse_Kinematics.TORSO_HIP_X, 0, Inverse_Kinematics.TORSO_HIP_Z)
def head_to_hip_transform(self, coords, is_batch=False):
'''
Convert cartesian coordinates that are relative to head to coordinates that are relative the center of both hip joints
Parameters
----------
coords : array_like
One 3D position or list of 3D positions
is_batch : `bool`
Indicates if coords is a batch of 3D positions
Returns
-------
coord : `list` or ndarray
A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned
'''
coords_rel_torso = self.robot.head_to_body_part_transform( "torso", coords, is_batch )
return self.torso_to_hip_transform(coords_rel_torso, is_batch)
def get_body_part_pos_relative_to_hip(self, body_part_name):
''' Get body part position relative to the center of both hip joints '''
bp_rel_head = self.robot.body_parts[body_part_name].transform.get_translation()
return self.head_to_hip_transform(bp_rel_head)
def get_ankle_pos_relative_to_hip(self, is_left):
''' Internally calls get_body_part_pos_relative_to_hip() '''
return self.get_body_part_pos_relative_to_hip("lankle" if is_left else "rankle")
def get_linear_leg_trajectory(self, is_left:bool, p1, p2=None, foot_ori3d=(0,0,0), dynamic_pose:bool=True, resolution=100):
'''
Compute leg trajectory so that the ankle moves linearly between two 3D points (relative to hip)
Parameters
----------
is_left : `bool`
set to True to select left leg, False to select right leg
p1 : array_like, length 3
if p2 is None:
p1 is the target position (relative to hip), and the initial point is given by the ankle's current position
if p2 is not None:
p1 is the initial point (relative to hip)
p2 : array_like, length 3 / `None`
target position (relative to hip) or None (see p1)
foot_ori3d : array_like, length 3
rotation around x,y,z (rotation around x & y are biases, relative to a vertical pose, or dynamic pose, if enabled)
dynamic_pose : `bool`
enable dynamic feet rotation to be parallel to the ground, based on IMU
resolution : int
interpolation resolution; more resolution is always better, but it takes more time to compute;
having more points does not make the movement slower, because if there are excessive points they are removed
during the analytical optimization
Returns
-------
trajecory : `tuple`
indices, [[values_1,error_codes_1], [values_2,error_codes_2], ...]
See leg() for further details
'''
if p2 is None:
p2 = np.asarray(p1, float)
p1 = self.get_body_part_pos_relative_to_hip('lankle' if is_left else 'rankle')
else:
p1 = np.asarray(p1, float)
p2 = np.asarray(p2, float)
vec = (p2 - p1) / resolution
hip_points = [p1 + vec * i for i in range(1,resolution+1)]
interpolation = [self.leg(p, foot_ori3d, is_left, dynamic_pose) for p in hip_points]
indices = [2,4,6,8,10,12] if is_left else [3,5,7,9,11,13]
last_joint_values = self.robot.joints_position[indices[0:4]] #exclude feet joints to compute ankle trajectory
next_step = interpolation[0]
trajectory = []
for p in interpolation[1:-1]:
if np.any(np.abs(p[1][0:4]-last_joint_values) > 7.03):
trajectory.append(next_step[1:3])
last_joint_values = next_step[1][0:4]
next_step = p
else:
next_step = p
trajectory.append(interpolation[-1][1:3])
return indices, trajectory
def leg(self, ankle_pos3d, foot_ori3d, is_left:bool, dynamic_pose:bool):
'''
Compute inverse kinematics for the leg, considering as input the relative 3D position of the ankle and 3D orientation* of the foot
*the yaw can be controlled directly, but the pitch and roll are biases (see below)
Parameters
----------
ankle_pos3d : array_like, length 3
(x,y,z) position of ankle in 3D, relative to the center of both hip joints
foot_ori3d : array_like, length 3
rotation around x,y,z (rotation around x & y are biases, relative to a vertical pose, or dynamic pose, if enabled)
is_left : `bool`
set to True to select left leg, False to select right leg
dynamic_pose : `bool`
enable dynamic feet rotation to be parallel to the ground, based on IMU
Returns
-------
indices : `list`
indices of computed joints
values : `list`
values of computed joints
error_codes : `list`
list of error codes
Error codes:
(-1) Foot is too far (unreachable)
(x) Joint x is out of range
'''
error_codes = []
leg_y_dev, upper_leg_height, upper_leg_depth, lower_leg_len, knee_extra_angle, _ = self.NAO_SPECS
sign = -1 if is_left else 1
# Then we translate to origin of leg by shifting the y coordinate
ankle_pos3d = np.asarray(ankle_pos3d) + (0,sign*leg_y_dev,0)
# First we rotate the leg, then we rotate the coordinates to abstract from the rotation
ankle_pos3d = Matrix_3x3().rotate_z_deg(-foot_ori3d[2]).multiply(ankle_pos3d)
# Use geometric solution to compute knee angle and foot pitch
dist = np.linalg.norm(ankle_pos3d) #dist hip <-> ankle
sq_dist = dist * dist
sq_upper_leg_h = upper_leg_height * upper_leg_height
sq_lower_leg_l = lower_leg_len * lower_leg_len
sq_upper_leg_l = upper_leg_depth * upper_leg_depth + sq_upper_leg_h
upper_leg_len = sqrt(sq_upper_leg_l)
knee = M.acos((sq_upper_leg_l + sq_lower_leg_l - sq_dist)/(2 * upper_leg_len * lower_leg_len)) + knee_extra_angle # Law of cosines
foot = M.acos((sq_lower_leg_l + sq_dist - sq_upper_leg_l)/(2 * lower_leg_len * dist)) # foot perpendicular to vec(origin->ankle_pos)
# Check if target is reachable
if dist > upper_leg_len + lower_leg_len:
error_codes.append(-1)
# Knee and foot
knee_angle = pi - knee
foot_pitch = foot - atan(ankle_pos3d[0] / np.linalg.norm(ankle_pos3d[1:3]))
foot_roll = atan(ankle_pos3d[1] / min(-0.05, ankle_pos3d[2])) * -sign # avoid instability of foot roll (not relevant above -0.05m)
# Raw hip angles if all joints were straightforward
raw_hip_yaw = foot_ori3d[2]
raw_hip_pitch = foot_pitch - knee_angle
raw_hip_roll = -sign * foot_roll
# Rotate 45deg due to yaw joint orientation, then rotate yaw, roll and pitch
m = Matrix_3x3().rotate_y_rad(raw_hip_pitch).rotate_x_rad(raw_hip_roll).rotate_z_deg(raw_hip_yaw).rotate_x_deg(-45*sign)
# Get actual hip angles considering the yaw joint orientation
hip_roll = (pi/4) - (sign * asin(m.m[1,2])) #Add pi/4 due to 45deg rotation
hip_pitch = - atan2(m.m[0,2],m.m[2,2])
hip_yaw = sign * atan2(m.m[1,0],m.m[1,1])
# Convert rad to deg
values = np.array([hip_yaw,hip_roll,hip_pitch,-knee_angle,foot_pitch,foot_roll]) * 57.2957795 #rad to deg
# Set feet rotation bias (based on vertical pose, or dynamic_pose)
values[4] -= foot_ori3d[1]
values[5] -= foot_ori3d[0] * sign
indices = [2,4,6,8,10,12] if is_left else [3,5,7,9,11,13]
if dynamic_pose:
# Rotation of torso in relation to foot
m : Matrix_3x3 = Matrix_3x3.from_rotation_deg((self.robot.imu_torso_roll, self.robot.imu_torso_pitch, 0))
m.rotate_z_deg(foot_ori3d[2], True)
roll = m.get_roll_deg()
pitch = m.get_pitch_deg()
# Simple balance algorithm
correction = 1 #correction to motivate a vertical torso (in degrees)
roll = 0 if abs(roll) < correction else roll - np.copysign(correction,roll)
pitch = 0 if abs(pitch) < correction else pitch - np.copysign(correction,pitch)
values[4] += pitch
values[5] += roll * sign
# Check and limit range of joints
for i in range(len(indices)):
if values[i] < self.robot.joints_info[indices[i]].min or values[i] > self.robot.joints_info[indices[i]].max:
error_codes.append(indices[i])
values[i] = np.clip(values[i], self.robot.joints_info[indices[i]].min, self.robot.joints_info[indices[i]].max)
return indices, values, error_codes

361
math_ops/Math_Ops.py Normal file
View File

@@ -0,0 +1,361 @@
from math import acos, asin, atan2, cos, pi, sin, sqrt
import numpy as np
import sys
try:
GLOBAL_DIR = sys._MEIPASS # temporary folder with libs & data files
except:
GLOBAL_DIR = "."
class Math_Ops():
'''
This class provides general mathematical operations that are not directly available through numpy
'''
@staticmethod
def deg_sph2cart(spherical_vec):
''' Converts SimSpark's spherical coordinates in degrees to cartesian coordinates '''
r = spherical_vec[0]
h = spherical_vec[1] * pi / 180
v = spherical_vec[2] * pi / 180
return np.array([r * cos(v) * cos(h), r * cos(v) * sin(h), r * sin(v)])
@staticmethod
def deg_sin(deg_angle):
''' Returns sin of degrees '''
return sin(deg_angle * pi / 180)
@staticmethod
def deg_cos(deg_angle):
''' Returns cos of degrees '''
return cos(deg_angle * pi / 180)
@staticmethod
def to_3d(vec_2d, value=0) -> np.ndarray:
''' Returns new 3d vector from 2d vector '''
return np.append(vec_2d,value)
@staticmethod
def to_2d_as_3d(vec_3d) -> np.ndarray:
''' Returns new 3d vector where the 3rd dimension is zero '''
vec_2d_as_3d = np.copy(vec_3d)
vec_2d_as_3d[2] = 0
return vec_2d_as_3d
@staticmethod
def normalize_vec(vec) -> np.ndarray:
''' Divides vector by its length '''
size = np.linalg.norm(vec)
if size == 0: return vec
return vec / size
@staticmethod
def get_active_directory(dir:str) -> str:
global GLOBAL_DIR
return GLOBAL_DIR + dir
@staticmethod
def acos(val):
''' arccosine function that limits input '''
return acos( np.clip(val,-1,1) )
@staticmethod
def asin(val):
''' arcsine function that limits input '''
return asin( np.clip(val,-1,1) )
@staticmethod
def normalize_deg(val):
''' normalize val in range [-180,180[ '''
return (val + 180.0) % 360 - 180
@staticmethod
def normalize_rad(val):
''' normalize val in range [-pi,pi[ '''
return (val + pi) % (2*pi) - pi
@staticmethod
def deg_to_rad(val):
''' convert degrees to radians '''
return val * 0.01745329251994330
@staticmethod
def rad_to_deg(val):
''' convert radians to degrees '''
return val * 57.29577951308232
@staticmethod
def vector_angle(vector, is_rad=False):
''' angle (degrees or radians) of 2D vector '''
if is_rad:
return atan2(vector[1], vector[0])
else:
return atan2(vector[1], vector[0]) * 180 / pi
@staticmethod
def vectors_angle(vec1, vec2, is_rad=False):
''' get angle between vectors (degrees or radians) '''
ang_rad = acos(np.dot(Math_Ops.normalize_vec(vec1),Math_Ops.normalize_vec(vec2)))
return ang_rad if is_rad else ang_rad * 180 / pi
@staticmethod
def vector_from_angle(angle, is_rad=False):
''' unit vector with direction given by `angle` '''
if is_rad:
return np.array([cos(angle), sin(angle)], float)
else:
return np.array([Math_Ops.deg_cos(angle), Math_Ops.deg_sin(angle)], float)
@staticmethod
def target_abs_angle(pos2d, target, is_rad=False):
''' angle (degrees or radians) of vector (target-pos2d) '''
if is_rad:
return atan2(target[1]-pos2d[1], target[0]-pos2d[0])
else:
return atan2(target[1]-pos2d[1], target[0]-pos2d[0]) * 180 / pi
@staticmethod
def target_rel_angle(pos2d, ori, target, is_rad=False):
''' relative angle (degrees or radians) of target if we're located at 'pos2d' with orientation 'ori' (degrees or radians) '''
if is_rad:
return Math_Ops.normalize_rad( atan2(target[1]-pos2d[1], target[0]-pos2d[0]) - ori )
else:
return Math_Ops.normalize_deg( atan2(target[1]-pos2d[1], target[0]-pos2d[0]) * 180 / pi - ori )
@staticmethod
def rotate_2d_vec(vec, angle, is_rad=False):
''' rotate 2D vector anticlockwise around the origin by `angle` '''
cos_ang = cos(angle) if is_rad else cos(angle * pi / 180)
sin_ang = sin(angle) if is_rad else sin(angle * pi / 180)
return np.array([cos_ang*vec[0]-sin_ang*vec[1], sin_ang*vec[0]+cos_ang*vec[1]])
@staticmethod
def distance_point_to_line(p:np.ndarray, a:np.ndarray, b:np.ndarray):
'''
Distance between point p and 2d line 'ab' (and side where p is)
Parameters
----------
a : ndarray
2D point that defines line
b : ndarray
2D point that defines line
p : ndarray
2D point
Returns
-------
distance : float
distance between line and point
side : str
if we are at a, looking at b, p may be at our "left" or "right"
'''
line_len = np.linalg.norm(b-a)
if line_len == 0: # assumes vertical line
dist = sdist = np.linalg.norm(p-a)
else:
sdist = np.cross(b-a,p-a)/line_len
dist = abs(sdist)
return dist, "left" if sdist>0 else "right"
@staticmethod
def distance_point_to_segment(p:np.ndarray, a:np.ndarray, b:np.ndarray):
''' Distance from point p to 2d line segment 'ab' '''
ap = p-a
ab = b-a
ad = Math_Ops.vector_projection(ap,ab)
# Is d in ab? We can find k in (ad = k * ab) without computing any norm
# we use the largest dimension of ab to avoid division by 0
k = ad[0]/ab[0] if abs(ab[0])>abs(ab[1]) else ad[1]/ab[1]
if k <= 0: return np.linalg.norm(ap)
elif k >= 1: return np.linalg.norm(p-b)
else: return np.linalg.norm(p-(ad + a)) # p-d
@staticmethod
def distance_point_to_ray(p:np.ndarray, ray_start:np.ndarray, ray_direction:np.ndarray):
''' Distance from point p to 2d ray '''
rp = p-ray_start
rd = Math_Ops.vector_projection(rp,ray_direction)
# Is d in ray? We can find k in (rd = k * ray_direction) without computing any norm
# we use the largest dimension of ray_direction to avoid division by 0
k = rd[0]/ray_direction[0] if abs(ray_direction[0])>abs(ray_direction[1]) else rd[1]/ray_direction[1]
if k <= 0: return np.linalg.norm(rp)
else: return np.linalg.norm(p-(rd + ray_start)) # p-d
@staticmethod
def closest_point_on_ray_to_point(p:np.ndarray, ray_start:np.ndarray, ray_direction:np.ndarray):
''' Point on ray closest to point p '''
rp = p-ray_start
rd = Math_Ops.vector_projection(rp,ray_direction)
# Is d in ray? We can find k in (rd = k * ray_direction) without computing any norm
# we use the largest dimension of ray_direction to avoid division by 0
k = rd[0]/ray_direction[0] if abs(ray_direction[0])>abs(ray_direction[1]) else rd[1]/ray_direction[1]
if k <= 0: return ray_start
else: return rd + ray_start
@staticmethod
def does_circle_intersect_segment(p:np.ndarray, r, a:np.ndarray, b:np.ndarray):
''' Returns true if circle (center p, radius r) intersect 2d line segment '''
ap = p-a
ab = b-a
ad = Math_Ops.vector_projection(ap,ab)
# Is d in ab? We can find k in (ad = k * ab) without computing any norm
# we use the largest dimension of ab to avoid division by 0
k = ad[0]/ab[0] if abs(ab[0])>abs(ab[1]) else ad[1]/ab[1]
if k <= 0: return np.dot(ap,ap) <= r*r
elif k >= 1: return np.dot(p-b,p-b) <= r*r
dp = p-(ad + a)
return np.dot(dp,dp) <= r*r
@staticmethod
def vector_projection(a:np.ndarray, b:np.ndarray):
''' Vector projection of a onto b '''
b_dot = np.dot(b,b)
return b * np.dot(a,b) / b_dot if b_dot != 0 else b
@staticmethod
def do_noncollinear_segments_intersect(a,b,c,d):
'''
Check if 2d line segment 'ab' intersects with noncollinear 2d line segment 'cd'
Explanation: https://www.geeksforgeeks.org/check-if-two-given-line-segments-intersect/
'''
ccw = lambda a,b,c: (c[1]-a[1]) * (b[0]-a[0]) > (b[1]-a[1]) * (c[0]-a[0])
return ccw(a,c,d) != ccw(b,c,d) and ccw(a,b,c) != ccw(a,b,d)
@staticmethod
def intersection_segment_opp_goal(a:np.ndarray, b:np.ndarray):
''' Computes the intersection point of 2d segment 'ab' and the opponents' goal (front line) '''
vec_x = b[0]-a[0]
# Collinear intersections are not accepted
if vec_x == 0: return None
k = (15.01-a[0])/vec_x
# No collision
if k < 0 or k > 1: return None
intersection_pt = a + (b-a) * k
if -1.01 <= intersection_pt[1] <= 1.01:
return intersection_pt
else:
return None
@staticmethod
def intersection_circle_opp_goal(p:np.ndarray, r):
'''
Computes the intersection segment of circle (center p, radius r) and the opponents' goal (front line)
Only the y coordinates are returned since the x coordinates are always equal to 15
'''
x_dev = abs(15-p[0])
if x_dev > r:
return None # no intersection with x=15
y_dev = sqrt(r*r - x_dev*x_dev)
p1 = max(p[1] - y_dev, -1.01)
p2 = min(p[1] + y_dev, 1.01)
if p1 == p2:
return p1 # return the y coordinate of a single intersection point
elif p2 < p1:
return None # no intersection
else:
return p1, p2 # return the y coordinates of the intersection segment
@staticmethod
def distance_point_to_opp_goal(p:np.ndarray):
''' Distance between point 'p' and the opponents' goal (front line) '''
if p[1] < -1.01:
return np.linalg.norm( p-(15,-1.01) )
elif p[1] > 1.01:
return np.linalg.norm( p-(15, 1.01) )
else:
return abs(15-p[0])
@staticmethod
def circle_line_segment_intersection(circle_center, circle_radius, pt1, pt2, full_line=True, tangent_tol=1e-9):
""" Find the points at which a circle intersects a line-segment. This can happen at 0, 1, or 2 points.
:param circle_center: The (x, y) location of the circle center
:param circle_radius: The radius of the circle
:param pt1: The (x, y) location of the first point of the segment
:param pt2: The (x, y) location of the second point of the segment
:param full_line: True to find intersections along full line - not just in the segment. False will just return intersections within the segment.
:param tangent_tol: Numerical tolerance at which we decide the intersections are close enough to consider it a tangent
:return Sequence[Tuple[float, float]]: A list of length 0, 1, or 2, where each element is a point at which the circle intercepts a line segment.
Note: We follow: http://mathworld.wolfram.com/Circle-LineIntersection.html
"""
(p1x, p1y), (p2x, p2y), (cx, cy) = pt1, pt2, circle_center
(x1, y1), (x2, y2) = (p1x - cx, p1y - cy), (p2x - cx, p2y - cy)
dx, dy = (x2 - x1), (y2 - y1)
dr = (dx ** 2 + dy ** 2)**.5
big_d = x1 * y2 - x2 * y1
discriminant = circle_radius ** 2 * dr ** 2 - big_d ** 2
if discriminant < 0: # No intersection between circle and line
return []
else: # There may be 0, 1, or 2 intersections with the segment
intersections = [
(cx + (big_d * dy + sign * (-1 if dy < 0 else 1) * dx * discriminant**.5) / dr ** 2,
cy + (-big_d * dx + sign * abs(dy) * discriminant**.5) / dr ** 2)
for sign in ((1, -1) if dy < 0 else (-1, 1))] # This makes sure the order along the segment is correct
if not full_line: # If only considering the segment, filter out intersections that do not fall within the segment
fraction_along_segment = [
(xi - p1x) / dx if abs(dx) > abs(dy) else (yi - p1y) / dy for xi, yi in intersections]
intersections = [pt for pt, frac in zip(
intersections, fraction_along_segment) if 0 <= frac <= 1]
# If line is tangent to circle, return just one point (as both intersections have same location)
if len(intersections) == 2 and abs(discriminant) <= tangent_tol:
return [intersections[0]]
else:
return intersections
# adapted from https://stackoverflow.com/questions/3252194/numpy-and-line-intersections
@staticmethod
def get_line_intersection(a1, a2, b1, b2):
"""
Returns the point of intersection of the lines passing through a2,a1 and b2,b1.
a1: [x, y] a point on the first line
a2: [x, y] another point on the first line
b1: [x, y] a point on the second line
b2: [x, y] another point on the second line
"""
s = np.vstack([a1,a2,b1,b2]) # s for stacked
h = np.hstack((s, np.ones((4, 1)))) # h for homogeneous
l1 = np.cross(h[0], h[1]) # get first line
l2 = np.cross(h[2], h[3]) # get second line
x, y, z = np.cross(l1, l2) # point of intersection
if z == 0: # lines are parallel
return np.array([float('inf'), float('inf')])
return np.array([x/z, y/z],float)

351
math_ops/Matrix_3x3.py Normal file
View File

@@ -0,0 +1,351 @@
from math import asin, atan2, pi, sqrt
import numpy as np
import math
class Matrix_3x3():
def __init__(self, matrix = None) -> None:
'''
Constructor examples:
a = Matrix_3x3( ) # create identity matrix
b = Matrix_3x3( [[1,1,1],[2,2,2],[3,3,3]] ) # manually initialize matrix
c = Matrix_3x3( [1,1,1,2,2,2,3,3,3] ) # manually initialize matrix
d = Matrix_3x3( b ) # copy constructor
'''
if matrix is None:
self.m = np.identity(3)
elif type(matrix) == Matrix_3x3:
self.m = np.copy(matrix.m)
else:
self.m = np.asarray(matrix)
self.m.shape = (3,3) #reshape if needed, throw error if impossible
self.rotation_shortcuts={(1,0,0):self.rotate_x_rad, (-1, 0, 0):self._rotate_x_neg_rad,
(0,1,0):self.rotate_y_rad, ( 0,-1, 0):self._rotate_y_neg_rad,
(0,0,1):self.rotate_z_rad, ( 0, 0,-1):self._rotate_z_neg_rad}
@classmethod
def from_rotation_deg(cls, euler_vec):
'''
Create rotation matrix from Euler angles, in degrees.
Rotation order: RotZ*RotY*RotX
Parameters
----------
euler_vec : array_like, length 3
vector with Euler angles (x,y,z) aka (roll, pitch, yaw)
Example
----------
Matrix_3x3.from_rotation_deg((roll,pitch,yaw)) # Creates: RotZ(yaw)*RotY(pitch)*RotX(roll)
'''
mat = cls().rotate_z_deg(euler_vec[2], True).rotate_y_deg(euler_vec[1], True).rotate_x_deg(euler_vec[0], True)
return mat
def get_roll_deg(self):
''' Get angle around the x-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
if self.m[2,1] == 0 and self.m[2,2] == 0:
return 180
return atan2(self.m[2,1], self.m[2,2]) * 180 / pi
def get_pitch_deg(self):
''' Get angle around the y-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
return atan2(-self.m[2,0], sqrt(self.m[2,1]*self.m[2,1] + self.m[2,2]*self.m[2,2])) * 180 / pi
def get_yaw_deg(self):
''' Get angle around the z-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
if self.m[1,0] == 0 and self.m[0,0] == 0:
return atan2(self.m[0,1], self.m[1,1]) * 180 / pi
return atan2(self.m[1,0], self.m[0,0]) * 180 / pi
def get_inclination_deg(self):
''' Get inclination of z-axis in relation to reference z-axis '''
return 90 - (asin(self.m[2,2]) * 180 / pi)
def rotate_deg(self, rotation_vec, rotation_deg, in_place=False):
'''
Rotates the current rotation matrix
Parameters
----------
rotation_vec : array_like, length 3
rotation vector
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
return self.rotate_rad(rotation_vec, rotation_deg * (pi/180) , in_place)
def rotate_rad(self, rotation_vec, rotation_rad, in_place=False):
'''
Rotates the current rotation matrix
Parameters
----------
rotation_vec : array_like, length 3
rotation vector
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
if rotation_rad == 0: return
shortcut = self.rotation_shortcuts.get(tuple(a for a in rotation_vec))
if shortcut:
return shortcut(rotation_rad, in_place)
c = math.cos(rotation_rad)
c1 = 1 - c
s = math.sin(rotation_rad)
x = rotation_vec[0]
y = rotation_vec[1]
z = rotation_vec[2]
xxc1 = x * x * c1
yyc1 = y * y * c1
zzc1 = z * z * c1
xyc1 = x * y * c1
xzc1 = x * z * c1
yzc1 = y * z * c1
xs = x * s
ys = y * s
zs = z * s
mat = np.array([
[xxc1 + c, xyc1 - zs, xzc1 + ys],
[xyc1 + zs, yyc1 + c, yzc1 - xs],
[xzc1 - ys, yzc1 + xs, zzc1 + c]])
return self.multiply(mat, in_place)
def _rotate_x_neg_rad(self, rotation_rad, in_place=False):
self.rotate_x_rad(-rotation_rad, in_place)
def _rotate_y_neg_rad(self, rotation_rad, in_place=False):
self.rotate_y_rad(-rotation_rad, in_place)
def _rotate_z_neg_rad(self, rotation_rad, in_place=False):
self.rotate_z_rad(-rotation_rad, in_place)
def rotate_x_rad(self, rotation_rad, in_place=False):
'''
Rotates the current rotation matrix around the x-axis
Parameters
----------
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_3x3(self)
c = math.cos(rotation_rad)
s = math.sin(rotation_rad)
mat = np.array([
[1, 0, 0],
[0, c,-s],
[0, s, c]])
return self.multiply(mat, in_place)
def rotate_y_rad(self, rotation_rad, in_place=False):
'''
Rotates the current rotation matrix around the y-axis
Parameters
----------
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_3x3(self)
c = math.cos(rotation_rad)
s = math.sin(rotation_rad)
mat = np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]])
return self.multiply(mat, in_place)
def rotate_z_rad(self, rotation_rad, in_place=False):
'''
Rotates the current rotation matrix around the z-axis
Parameters
----------
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_3x3(self)
c = math.cos(rotation_rad)
s = math.sin(rotation_rad)
mat = np.array([
[ c,-s, 0],
[ s, c, 0],
[ 0, 0, 1]])
return self.multiply(mat, in_place)
def rotate_x_deg(self, rotation_deg, in_place=False):
'''
Rotates the current rotation matrix around the x-axis
Parameters
----------
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
return self.rotate_x_rad(rotation_deg * (pi/180), in_place)
def rotate_y_deg(self, rotation_deg, in_place=False):
'''
Rotates the current rotation matrix around the y-axis
Parameters
----------
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
return self.rotate_y_rad(rotation_deg * (pi/180), in_place)
def rotate_z_deg(self, rotation_deg, in_place=False):
'''
Rotates the current rotation matrix around the z-axis
Parameters
----------
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
return self.rotate_z_rad(rotation_deg * (pi/180), in_place)
def invert(self, in_place=False):
'''
Inverts the current rotation matrix
Parameters
----------
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_3x3
self is returned if in_place is True
'''
if in_place:
self.m = np.linalg.inv(self.m)
return self
else:
return Matrix_3x3(np.linalg.inv(self.m))
def multiply(self,mat, in_place=False, reverse_order=False):
'''
Multiplies the current rotation matrix by mat
Parameters
----------
mat : Matrix_3x3 or array_like
multiplier matrix or 3D vector
in_place: bool, optional
- True: the internal matrix is changed in-place
- False: a new matrix is returned and the current one is not changed (default)
reverse_order: bool, optional
- False: self * mat
- True: mat * self
Returns
-------
result : Matrix_3x3 | array_like
Matrix_3x3 is returned if mat is a matrix (self is returned if in_place is True);
a 3D vector is returned if mat is a vector
'''
# get array from matrix object or convert to numpy array (if needed)
mat = mat.m if type(mat) == Matrix_3x3 else np.asarray(mat)
a,b = (mat, self.m) if reverse_order else (self.m, mat)
if mat.ndim == 1:
return np.matmul(a, b) # multiplication by 3D vector
elif in_place:
np.matmul(a, b, self.m) # multiplication by matrix, in place
return self
else: # multiplication by matrix, return new Matrix_3x3
return Matrix_3x3(np.matmul(a, b))

441
math_ops/Matrix_4x4.py Normal file
View File

@@ -0,0 +1,441 @@
from math import asin, atan2, pi, sqrt
from math_ops.Math_Ops import Math_Ops as M
from math_ops.Matrix_3x3 import Matrix_3x3
import numpy as np
import math
class Matrix_4x4():
def __init__(self, matrix = None) -> None:
'''
Constructor examples:
a = Matrix_4x4( ) # create identity matrix
b = Matrix_4x4( [[1,1,1,1],[2,2,2,2],[3,3,3,3],[4,4,4,4]] ) # manually initialize matrix
c = Matrix_4x4( [1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4] ) # manually initialize matrix
d = Matrix_4x4( b ) # copy constructor
'''
if matrix is None:
self.m = np.identity(4)
elif type(matrix) == Matrix_4x4:
self.m = np.copy(matrix.m)
elif type(matrix) == Matrix_3x3:
self.m = np.identity(4)
self.m[0:3,0:3] = matrix.m
else:
self.m = np.asarray(matrix)
self.m.shape = (4,4) #reshape if needed, throw error if impossible
@classmethod
def from_translation(cls, translation_vec):
'''
Create transformation matrix from translation_vec translation
e.g. Matrix_4x4.from_translation((a,b,c))
output: [[1,0,0,a],[0,1,0,b],[0,0,1,c],[0,0,0,1]]
'''
mat = np.identity(4)
mat[0:3,3] = translation_vec
return cls(mat)
@classmethod
def from_3x3_and_translation(cls, mat3x3:Matrix_3x3, translation_vec):
'''
Create transformation matrix from rotation matrix (3x3) and translation
e.g. Matrix_4x4.from_3x3_and_translation(r,(a,b,c))
output: [[r00,r01,r02,a],[r10,r11,r12,b],[r20,r21,r22,c],[0,0,0,1]]
'''
mat = np.identity(4)
mat[0:3,0:3] = mat3x3.m
mat[0:3,3] = translation_vec
return cls(mat)
def translate(self, translation_vec, in_place=False):
'''
Translates the current transformation matrix
Parameters
----------
translation_vec : array_like, length 3
translation vector
in_place: bool, optional
* True: the internal matrix is changed in-place
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
vec = np.array([*translation_vec,1])# conversion to 4D vector
np.matmul(self.m, vec, out=vec) # compute only 4th column
if in_place:
self.m[:,3] = vec
return self
else:
ret = Matrix_4x4(self.m)
ret.m[:,3] = vec
return ret
def get_translation(self):
''' Get translation vector (x,y,z) '''
return self.m[0:3,3] # return view
def get_x(self):
return self.m[0,3]
def get_y(self):
return self.m[1,3]
def get_z(self):
return self.m[2,3]
def get_rotation_4x4(self):
''' Get Matrix_4x4 without translation '''
mat = Matrix_4x4(self)
mat.m[0:3,3] = 0
return mat
def get_rotation(self):
''' Get rotation Matrix_3x3 '''
return Matrix_3x3(self.m[0:3,0:3])
def get_distance(self):
''' Get translation vector length '''
return np.linalg.norm(self.m[0:3,3])
def get_roll_deg(self):
''' Get angle around the x-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
if self.m[2,1] == 0 and self.m[2,2] == 0:
return 180
return atan2(self.m[2,1], self.m[2,2]) * 180 / pi
def get_pitch_deg(self):
''' Get angle around the y-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
return atan2(-self.m[2,0], sqrt(self.m[2,1]*self.m[2,1] + self.m[2,2]*self.m[2,2])) * 180 / pi
def get_yaw_deg(self):
''' Get angle around the z-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot '''
if self.m[1,0] == 0 and self.m[0,0] == 0:
return atan2(self.m[0,1], self.m[1,1]) * 180 / pi
return atan2(self.m[1,0], self.m[0,0]) * 180 / pi
def get_inclination_deg(self):
''' Get inclination of z-axis in relation to reference z-axis '''
return 90 - (asin(np.clip(self.m[2,2],-1,1)) * 180 / pi)
def rotate_deg(self, rotation_vec, rotation_deg, in_place=False):
'''
Rotates the current transformation matrix
Parameters
----------
rotation_vec : array_like, length 3
rotation vector
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
return self.rotate_rad(rotation_vec, rotation_deg * (pi/180) , in_place)
def rotate_rad(self, rotation_vec, rotation_rad, in_place=False):
'''
Rotates the current transformation matrix
Parameters
----------
rotation_vec : array_like, length 3
rotation vector
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_4x4(self)
# shortcuts for rotation around 1 axis
if rotation_vec[0]==0:
if rotation_vec[1]==0:
if rotation_vec[2]==1:
return self.rotate_z_rad(rotation_rad, in_place)
elif rotation_vec[2]==-1:
return self.rotate_z_rad(-rotation_rad, in_place)
elif rotation_vec[2]==0:
if rotation_vec[1]==1:
return self.rotate_y_rad(rotation_rad, in_place)
elif rotation_vec[1]==-1:
return self.rotate_y_rad(-rotation_rad, in_place)
elif rotation_vec[1]==0 and rotation_vec[2]==0:
if rotation_vec[0]==1:
return self.rotate_x_rad(rotation_rad, in_place)
elif rotation_vec[0]==-1:
return self.rotate_x_rad(-rotation_rad, in_place)
c = math.cos(rotation_rad)
c1 = 1 - c
s = math.sin(rotation_rad)
x = rotation_vec[0]
y = rotation_vec[1]
z = rotation_vec[2]
xxc1 = x * x * c1
yyc1 = y * y * c1
zzc1 = z * z * c1
xyc1 = x * y * c1
xzc1 = x * z * c1
yzc1 = y * z * c1
xs = x * s
ys = y * s
zs = z * s
mat = np.array([
[xxc1 + c, xyc1 - zs, xzc1 + ys, 0],
[xyc1 + zs, yyc1 + c, yzc1 - xs, 0],
[xzc1 - ys, yzc1 + xs, zzc1 + c, 0],
[0, 0, 0, 1]])
return self.multiply(mat, in_place)
def rotate_x_rad(self, rotation_rad, in_place=False):
'''
Rotates the current transformation matrix around the x-axis
Parameters
----------
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_4x4(self)
c = math.cos(rotation_rad)
s = math.sin(rotation_rad)
mat = np.array([
[1, 0, 0, 0],
[0, c,-s, 0],
[0, s, c, 0],
[0, 0, 0, 1]])
return self.multiply(mat, in_place)
def rotate_y_rad(self, rotation_rad, in_place=False):
'''
Rotates the current transformation matrix around the y-axis
Parameters
----------
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_4x4(self)
c = math.cos(rotation_rad)
s = math.sin(rotation_rad)
mat = np.array([
[ c, 0, s, 0],
[ 0, 1, 0, 0],
[-s, 0, c, 0],
[ 0, 0, 0, 1]])
return self.multiply(mat, in_place)
def rotate_z_rad(self, rotation_rad, in_place=False):
'''
Rotates the current transformation matrix around the z-axis
Parameters
----------
rotation_rad : float
rotation in radians
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
if rotation_rad == 0:
return self if in_place else Matrix_4x4(self)
c = math.cos(rotation_rad)
s = math.sin(rotation_rad)
mat = np.array([
[ c,-s, 0, 0],
[ s, c, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
return self.multiply(mat, in_place)
def rotate_x_deg(self, rotation_deg, in_place=False):
'''
Rotates the current transformation matrix around the x-axis
Parameters
----------
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
return self.rotate_x_rad(rotation_deg * (pi/180), in_place)
def rotate_y_deg(self, rotation_deg, in_place=False):
'''
Rotates the current transformation matrix around the y-axis
Parameters
----------
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
return self.rotate_y_rad(rotation_deg * (pi/180), in_place)
def rotate_z_deg(self, rotation_deg, in_place=False):
'''
Rotates the current transformation matrix around the z-axis
Parameters
----------
rotation_rad : float
rotation in degrees
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
return self.rotate_z_rad(rotation_deg * (pi/180), in_place)
def invert(self, in_place=False):
'''
Inverts the current transformation matrix
Parameters
----------
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed
Returns
-------
result : Matrix_4x4
self is returned if in_place is True
'''
if in_place:
self.m = np.linalg.inv(self.m)
return self
else:
return Matrix_4x4(np.linalg.inv(self.m))
def multiply(self,mat, in_place=False):
'''
Multiplies the current transformation matrix by mat
Parameters
----------
mat : Matrix_4x4 or array_like
multiplier matrix or 3D vector
in_place: bool, optional
* True: the internal matrix is changed in-place (default)
* False: a new matrix is returned and the current one is not changed (if mat is a 4x4 matrix)
Returns
-------
result : Matrix_4x4 | array_like
Matrix_4x4 is returned if mat is a matrix (self is returned if in_place is True);
a 3D vector is returned if mat is a vector
'''
if type(mat) == Matrix_4x4:
mat = mat.m
else:
mat = np.asarray(mat) # conversion to array, if needed
if mat.ndim == 1: # multiplication by 3D vector
vec = np.append(mat,1) # conversion to 4D vector
return np.matmul(self.m, vec)[0:3] # conversion to 3D vector
if in_place:
np.matmul(self.m, mat, self.m)
return self
else:
return Matrix_4x4(np.matmul(self.m, mat))
def __call__(self,mat, is_spherical=False):
'''
Multiplies the current transformation matrix by mat and returns a new matrix or vector
Parameters
----------
mat : Matrix_4x4 or array_like
multiplier matrix or 3D vector
is_spherical : bool
only relevant if mat is a 3D vector, True if it uses spherical coordinates
Returns
-------
result : Matrix_4x4 | array_like
Matrix_4x4 is returned if mat is a matrix;
a 3D vector is returned if mat is a vector
'''
if is_spherical and mat.ndim == 1: mat = M.deg_sph2cart(mat)
return self.multiply(mat,False)

28
math_ops/Neural_Network.py Normal file
View File

@@ -0,0 +1,28 @@
import numpy as np
def run_mlp(obs, weights, activation_function="tanh"):
'''
Run multilayer perceptron using numpy
Parameters
----------
obs : ndarray
float32 array with neural network inputs
weights : list
list of MLP layers of type (bias, kernel)
activation_function : str
activation function for hidden layers
set to "none" to disable
'''
obs = obs.astype(np.float32, copy=False)
out = obs
for w in weights[:-1]: # for each hidden layer
out = np.matmul(w[1],out) + w[0]
if activation_function == "tanh":
np.tanh(out, out=out)
elif activation_function != "none":
raise NotImplementedError
return np.matmul(weights[-1][1],out) + weights[-1][0] # final layer

BIN
math_ops/__pycache__/Inverse_Kinematics.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
math_ops/__pycache__/Math_Ops.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
math_ops/__pycache__/Matrix_3x3.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
math_ops/__pycache__/Matrix_4x4.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
math_ops/__pycache__/Neural_Network.cpython-313.pyc Normal file
View File

Binary file not shown.