Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixes body kin rot

Browse Source
This commit is contained in:
Rune Harlyk
2025-10-10 18:08:20 +02:00
committed by Rune Harlyk
parent d3db2b3650
commit a198de05c2
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simulation/src/robot/kinematics.py
+6 -4
View File
@@ -123,8 +123,10 @@ class Kinematics:
cr, sr = np.cos(roll), np.sin(roll) cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch) cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw) cy, sy = np.cos(yaw), np.sin(yaw)
return np.array([ return np.array(
[
[cp * cy, -cp * sy, sp], [cp * cy, -cp * sy, sp],
[sr*sp*cy + cy*cr, -sr*sp*sy + cr*cy, -sr*cp], [sr * sp * cy + sy * cr, -sr * sp * sy + cr * cy, -sr * cp],
[sr*sy - sp*cr*cy, sr*cy + sp*sy*cr, cr*cp] [sr * sy - sp * cr * cy, sr * cy + sp * sy * cr, cr * cp],
]) ]
)