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

♻️ Update sim structure

Browse Source
This commit is contained in:
Rune Harlyk
2025-07-18 19:22:59 +02:00
committed by Rune Harlyk
parent 5a6f195f56
commit d3db2b3650
29 changed files with 1224 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simulation/simulation/environment.py
-56
View File
@@ -1,56 +0,0 @@
import pybullet as p
import pybullet_data
import numpy as np
from simulation.robot import QuadrupedRobot
from simulation.gui import GUI
roll_pitch_reward_weight = 0.1
class QuadrupedEnv:
def __init__(self, urdf_path):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
p.setTimeStep(1 / 240)
self.urdf_path = urdf_path
self.numSolverIterations = 50
self.setupWorld()
self.gui = GUI(self.robot.robot_id)
self.envStartState = p.saveState()
def setupWorld(self):
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=self.numSolverIterations)
p.setGravity(0, 0, -9.8)
self.plane_id = p.loadURDF("plane.urdf")
self.robot = QuadrupedRobot(self.urdf_path)
def reset(self):
p.restoreState(self.envStartState)
return self.robot.get_observation()
def step(self, action):
self.gui.update()
self.robot.apply_action(action)
p.stepSimulation()
obs = self.robot.get_observation()
reward = self.calculate_reward(obs)
done = self.is_done(obs)
return obs, reward, done
def calculate_reward(self, observation):
reward = 0
reward += (
-(abs(observation[0]) + abs(observation[1])) * roll_pitch_reward_weight
)
return reward
def is_done(self, observation):
roll, pitch = observation[0:2]
return abs(roll) > 0.5 or abs(pitch) > 0.5
simulation/simulation/kinematic.py
-148
View File
@@ -1,148 +0,0 @@
import numpy as np
class Kinematic:
def __init__(self) -> None:
self.l1 = 60.5
self.l2 = 10
self.l3 = 100.7
self.l4 = 118.5
self.L = 207.5
self.W = 78
def calculate_inverse_kinematics(self, omega, phi, psi, x, y, z, feet):
Tlf, Trf, Tlb, Trb = self.bodyIK(omega, phi, psi, x, y, z)
Q = np.linalg.inv(Tlf).dot(feet[0])[:3]
IK = self.legIK(*Q)
LF = (
np.rad2deg(np.pi / 2 - IK[0]),
np.rad2deg(np.pi / 3 - IK[1]),
np.rad2deg(np.pi - IK[2]),
)
Q = self.Ix.dot(np.linalg.inv(Trf)).dot(feet[1])[:3]
IK = self.legIK(*Q)
RF = (
np.rad2deg(np.pi / 2 + IK[0]),
np.rad2deg(2 * np.pi / 3 + IK[1]),
np.rad2deg(IK[2]),
)
Q = np.linalg.inv(Tlb).dot(feet[2])[:3]
IK = self.legIK(*Q)
LB = (
np.rad2deg(np.pi / 2 + (IK[0])),
np.rad2deg(np.pi / 3 - IK[1]),
np.rad2deg(np.pi - IK[2]),
)
Q = self.Ix.dot(np.linalg.inv(Trb)).dot(feet[3])[:3]
IK = self.legIK(*Q)
RB = (
np.rad2deg(np.pi / 2 - IK[0]),
np.rad2deg(2 * np.pi / 3 + IK[1]),
np.rad2deg(IK[2]),
)
return (LF, RF, LB, RB)
def bodyIK(self, omega, phi, psi, xm, ym, zm):
sHp = np.sin(np.pi / 2)
cHp = np.cos(np.pi / 2)
Rx = np.array(
[
[1, 0, 0, 0],
[0, np.cos(omega), -np.sin(omega), 0],
[0, np.sin(omega), np.cos(omega), 0],
[0, 0, 0, 1],
]
)
Ry = np.array(
[
[np.cos(phi), 0, np.sin(phi), 0],
[0, 1, 0, 0],
[-np.sin(phi), 0, np.cos(phi), 0],
[0, 0, 0, 1],
]
)
Rz = np.array(
[
[np.cos(psi), -np.sin(psi), 0, 0],
[np.sin(psi), np.cos(psi), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
]
)
Rxyz = Rx @ Ry @ Rz
T = np.array([[0, 0, 0, xm], [0, 0, 0, ym], [0, 0, 0, zm], [0, 0, 0, 0]])
Tm = T + Rxyz
return [
Tm
@ np.array(
[
[cHp, 0, sHp, self.L / 2],
[0, 1, 0, 0],
[-sHp, 0, cHp, self.W / 2],
[0, 0, 0, 1],
]
),
Tm
@ np.array(
[
[cHp, 0, sHp, self.L / 2],
[0, 1, 0, 0],
[-sHp, 0, cHp, -self.W / 2],
[0, 0, 0, 1],
]
),
Tm
@ np.array(
[
[cHp, 0, sHp, -self.L / 2],
[0, 1, 0, 0],
[-sHp, 0, cHp, self.W / 2],
[0, 0, 0, 1],
]
),
Tm
@ np.array(
[
[cHp, 0, sHp, -self.L / 2],
[0, 1, 0, 0],
[-sHp, 0, cHp, -self.W / 2],
[0, 0, 0, 1],
]
),
]
def legIK(self, x, y, z):
"""
x/y/z=Position of the Foot in Leg-Space
F=Length of shoulder-point to target-point on x/y only
G=length we need to reach to the point on x/y
H=3-Dimensional length we need to reach
"""
F = np.sqrt(x**2 + y**2 - self.l1**2)
G = F - self.l2
H = np.sqrt(G**2 + z**2)
theta1 = -np.atan2(y, x) - np.atan2(F, -self.l1)
D = (H**2 - self.l3**2 - self.l4**2) / (2 * self.l3 * self.l4)
theta3 = np.acos(D)
theta2 = np.atan2(z, G) - np.atan2(
self.l4 * np.sin(theta3), self.l3 + self.l4 * np.cos(theta3)
)
return (theta1, theta2, theta3)
simulation/simulation/robot.py
-41
View File
@@ -1,41 +0,0 @@
import pybullet as p
import numpy as np
class QuadrupedRobot:
def __init__(self, urdf_path):
self.urdf_path = urdf_path
self.load()
def load(self):
position = [0, 0, 0.3]
orientation = p.getQuaternionFromEuler([0, 0, 0])
self.robot_id = p.loadURDF(self.urdf_path, position, orientation)
def get_observation(self):
_, orientation = p.getBasePositionAndOrientation(self.robot_id)
orientation = p.getEulerFromQuaternion(orientation)[:2]
velocity, angular_velocity = p.getBaseVelocity(self.robot_id)
joint_states = p.getJointStates(
self.robot_id, range(p.getNumJoints(self.robot_id))
)
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
return np.concatenate(
[
orientation,
velocity,
angular_velocity,
joint_positions,
joint_velocities,
]
)
def apply_action(self, action):
for i, position in enumerate(action):
p.setJointMotorControl2(
bodyIndex=self.robot_id,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=position,
)