Skip to content

Commit

Permalink
Fixed contact friction and gains on example/sim
Browse files Browse the repository at this point in the history
  • Loading branch information
@senthurayyappan
senthurayyappan committed Jan 15, 2025
1 parent 298c06c commit 22d3ca7
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 64 deletions.
2 changes: 1 addition & 1 deletion examples/simulation/ball.xml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<body name="ball" pos="0 0 0.132" quat="1 0 0 0">
<freejoint name="ball"/>
<inertial pos="0 0 -0.0075" quat="0.5 0.5 -0.5 0.5" mass="0.524" diaginertia="0.073541512 0.07356916 0.073543931"/>
<geom name="Part-1-1-collision" type="sphere" size="0.132" rgba="1 0.55 0 1" contype="1" conaffinity="1" density="0" group="0"/>
<geom name="Part-1-1-collision" type="sphere" size="0.132" rgba="1 0.55 0 1" contype="1" conaffinity="1" density="0" group="0" solref="0.01 1" solimp="0.8 0.9 0.01"/>
<geom type="mesh" rgba="1 0.55 0 1" mesh="Part-1-1" conaffinity="0" condim="1" contype="0" density="0" group="1"/>
</body>
159 changes: 107 additions & 52 deletions examples/simulation/main.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import numpy as np
import optuna
from mods import modify_ballbot
from optuna.pruners import MedianPruner
from scipy.spatial.transform import Rotation
from transformations import compute_motor_torques

Expand All @@ -21,61 +22,99 @@

# Variable bounds (in mm and degrees)
WHEEL_DIAMETER_BOUNDS = (100, 120)
SPACER_HEIGHT_BOUNDS = (65, 120)
SPACER_HEIGHT_BOUNDS = (75, 120)
ALPHA_BOUNDS = (30, 55)

SIMULATION_DURATION = 10 # seconds to run each trial
SIMULATION_DURATION = 20 # seconds to run each trial

PERTURBATION_INCREASE = 0.2 # Amount of Newtons to increase perturbation by each time
PERTURBATION_REST = 10 # Time delay between perturbations - begins after 5 seconds

MIN_HEIGHT = 0.15 # minimum height before considering failure

# PID parameters for roll, pitch, and yaw
KP = 160.0
KI = 10.0
KD = 2.0
KP = 12.0
KI = 6.0
KD = 0.05

TORQUE_LIMIT_HIGH = 5.0
TORQUE_LIMIT_LOW = -5.0

TORQUE_OFFSET = 0.25

integral_error_x = 0.0
integral_error_y = 0.0
integral_error_z = 0.0
previous_error_x = 0.0
previous_error_y = 0.0
previous_error_z = 0.0

def apply_ball_force(data, force):
data.qfrc_applied[9:12] = force

def get_theta(data, degrees=False):
rot = Rotation.from_quat(data.qpos[3:7], scalar_first=True)
theta = rot.as_euler("XYZ", degrees=degrees)
return theta[0], theta[1], theta[2]

def control(data, alpha):
global integral_error_x, integral_error_y, previous_error_x, previous_error_y

global integral_error_x, integral_error_y, integral_error_z, previous_error_x, previous_error_y, previous_error_z

# Get current orientation
theta = get_theta(data)

# Calculate errors
error_x = 0.0 - theta[0]
error_y = 0.0 - theta[1]
error_z = 0.0 - theta[2]

# Update integral errors
integral_error_x += error_x * dt
integral_error_y += error_y * dt
integral_error_z += error_z * dt

# Calculate derivative errors
derivative_error_x = (error_x - previous_error_x) / dt
derivative_error_y = (error_y - previous_error_y) / dt
derivative_error_z = (error_z - previous_error_z) / dt

# Individual terms
Px_term = KP * error_x
Ix_term = KI * integral_error_x
Dx_term = KD * derivative_error_x
Py_term = KP * error_y
Iy_term = KI * integral_error_y
Dy_term = KD * derivative_error_y
Pz_term = KP * error_z
Iz_term = KI * integral_error_z
Dz_term = KD * derivative_error_z

# PID control calculations
tx = KP * error_x + KI * integral_error_x + KD * derivative_error_x
ty = KP * error_y + KI * integral_error_y + KD * derivative_error_y
tz = 0.0

# Saturate the torque values to be within [-3, 3] Nm
tx = np.clip(tx, -3.0, 3.0)
ty = np.clip(ty, -3.0, 3.0)
tz = np.clip(tz, -3.0, 3.0)
tx = Px_term + Ix_term + Dx_term
ty = Py_term + Iy_term + Dy_term
tz = -(Pz_term + Iz_term + Dz_term) # EJR thinks this should be negative

# Adding offset torque to increase sensitivity around zero
tx = tx + TORQUE_OFFSET * np.sign(tx)
ty = ty + TORQUE_OFFSET * np.sign(ty)
#tz = tz + TORQUE_OFFSET * np.sign(tz)

# Saturate the torque values to be within [TORQUE_LIMIT_LOW, TORQUE_LIMIT_HIGH] Nm
# XML file specifies actuators with limits at [-50 50]
tx = np.clip(tx, TORQUE_LIMIT_LOW, TORQUE_LIMIT_HIGH)
ty = np.clip(ty, TORQUE_LIMIT_LOW, TORQUE_LIMIT_HIGH)
tz = np.clip(tz, TORQUE_LIMIT_LOW, TORQUE_LIMIT_HIGH)
#tz = 0.0 # Commenting out the removal of z-axis control for now

# Update previous errors
previous_error_x = error_x
previous_error_y = error_y
previous_error_z = error_z

# Compute motor torques
t1, t2, t3 = compute_motor_torques(np.deg2rad(alpha), tx, ty, tz)

data.ctrl[0] = t1
data.ctrl[1] = t2
data.ctrl[2] = t3
Expand Down Expand Up @@ -109,49 +148,64 @@ def objective(trial):

viewer = mujoco.viewer.launch_passive(model, data)
try:
while True:
timesteps = 0
max_timesteps = int(SIMULATION_DURATION / model.opt.timestep)
total_angle_error = 0
# reset global PID error values
global integral_error_x, integral_error_y, previous_error_x, previous_error_y
integral_error_x = 0.0
integral_error_y = 0.0
previous_error_x = 0.0
previous_error_y = 0.0
timesteps = 0
max_timesteps = int(SIMULATION_DURATION / model.opt.timestep)
total_angle_error = 0

i = 0
j = 0.0
direction = [0, 0, 0]

# reset global PID error values
global integral_error_x, integral_error_y, integral_error_z
global previous_error_x, previous_error_y, previous_error_z
integral_error_x = 0.0
integral_error_y = 0.0
integral_error_z = 0.0
previous_error_x = 0.0
previous_error_y = 0.0
previous_error_z = 0.0

# Reset data for a new trial
mujoco.mj_resetData(model, data)

# Reset data for a new trial
mujoco.mj_resetData(model, data)
while timesteps < max_timesteps and viewer.is_running():
i += 1
mujoco.mj_step(model, data)

while timesteps < max_timesteps and viewer.is_running():
mujoco.mj_step(model, data)
if data.time > 0.3:
control(data, alpha)

if data.time > 0.3:
control(data, alpha)
if data.time > 5 and i % (FREQUENCY*PERTURBATION_REST) == 0:
j += 1
LOGGER.info("Applying perturbation...")
direction = np.random.rand(3)
direction[2] = 0
force = direction * j * PERTURBATION_INCREASE
apply_ball_force(data, force)

if data.body("ballbot").xpos[2] < MIN_HEIGHT:
print("Ballbot fell below minimum height, ending trial.")
break

roll, pitch, _ = get_theta(data)
angle_error = np.sqrt(roll**2 + pitch**2)
total_angle_error += angle_error
if data.body("ballbot").xpos[2] < MIN_HEIGHT:
LOGGER.info("Ballbot fell below minimum height, ending trial.")
break

timesteps += 1
roll, pitch, yaw = get_theta(data)
angle_error = np.sqrt(roll**2 + pitch**2 + yaw**2)
total_angle_error += angle_error

viewer.sync()
timesteps += 1

if timesteps == 0:
print("No timesteps executed, returning infinity.")
return float("inf")
viewer.sync()

avg_angle_error = total_angle_error / timesteps
objective_value = -timesteps * 0.5 + avg_angle_error * 10
if timesteps == 0:
LOGGER.info("No timesteps executed, returning infinity.")
return float("inf")

break
avg_angle_error = total_angle_error / timesteps
objective_value = -timesteps * 0.5 + avg_angle_error * 10

except Exception as e:
print(f"An error occurred: {e}")
LOGGER.info(f"An error occurred: {e}")
return float("inf")
finally:
viewer.close()
Expand All @@ -171,12 +225,13 @@ def objective(trial):
elements = client.get_elements(doc.did, doc.wtype, doc.wid)
variables = client.get_variables(doc.did, doc.wid, elements["variables"].id)

study = optuna.create_study(direction="minimize")
study.optimize(objective, n_trials=10)
pruner = MedianPruner(n_startup_trials=5, n_warmup_steps=10)
study = optuna.create_study(direction="minimize", pruner=pruner)
study.optimize(objective, n_trials=20)

print("\nOptimization finished!")
print("Best trial:")
print(f" Value: {-study.best_trial.value}")
print(" Params:")
LOGGER.info("\nOptimization finished!")
LOGGER.info("Best trial:")
LOGGER.info(f" Value: {-study.best_trial.value}")
LOGGER.info(" Params:")
for key, value in study.best_trial.params.items():
print(f" {key}: {value}")
LOGGER.info(f" {key}: {value}")
36 changes: 28 additions & 8 deletions examples/simulation/mods.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,23 +28,23 @@ def modify_ballbot(ballbot: Robot) -> Robot:
actuator_name="motor-1",
joint_name="Revolute-1",
ctrl_limited=True,
ctrl_range=(-3, 3),
ctrl_range=(-50, 50),
add_encoder=True,
add_force_sensor=True,
)
ballbot.add_actuator(
actuator_name="motor-2",
joint_name="Revolute-2",
ctrl_limited=True,
ctrl_range=(-3, 3),
ctrl_range=(-50, 50),
add_encoder=True,
add_force_sensor=True,
)
ballbot.add_actuator(
actuator_name="motor-3",
joint_name="Revolute-3",
ctrl_limited=True,
ctrl_range=(-3, 3),
ctrl_range=(-50, 50),
add_encoder=True,
add_force_sensor=True,
)
Expand All @@ -56,28 +56,48 @@ def modify_ballbot(ballbot: Robot) -> Robot:
# Add sensor
ballbot.add_sensor(
name="imu",
sensor=IMU(name="imu", objtype="site", objname="imu", noise=0.001),
sensor=IMU(name="imu", objtype="site", objname="imu"),
)
ballbot.add_sensor(
name="gyro-1",
sensor=Gyro(name="gyro-1", site="imu", noise=0.001, cutoff=34.9),
sensor=Gyro(name="gyro-1", site="imu"),
)

contact = ET.Element("contact")
pair_1 = ET.SubElement(contact, "pair")
pair_1.set("geom1", "Part-2-3-collision")
pair_1.set("geom2", "Part-1-1-collision")
pair_1.set("friction", "0.3 0.3 0.005 0.9 0.9")
pair_1.set("friction", "1.75 2.5 0.001 0.001 0.001")

pair_2 = ET.SubElement(contact, "pair")
pair_2.set("geom1", "Part-2-2-collision")
pair_2.set("geom2", "Part-1-1-collision")
pair_2.set("friction", "0.3 0.3 0.005 0.9 0.9")
pair_2.set("friction", "1.75 2.5 0.001 0.001 0.001")

pair_3 = ET.SubElement(contact, "pair")
pair_3.set("geom1", "Part-2-1-collision")
pair_3.set("geom2", "Part-1-1-collision")
pair_3.set("friction", "0.3 0.3 0.005 0.9 0.9")
pair_3.set("friction", "1.75 2.5 0.001 0.001 0.001")

pair_4 = ET.SubElement(contact, "pair")
pair_4.set("geom1", "Part-2-3-collision")
pair_4.set("geom2", "floor")
pair_4.set("friction", "0.01 0.9 0.001 0.001 0.001")

pair_5 = ET.SubElement(contact, "pair")
pair_5.set("geom1", "Part-2-2-collision")
pair_5.set("geom2", "floor")
pair_5.set("friction", "0.01 0.9 0.001 0.001 0.001")

pair_6 = ET.SubElement(contact, "pair")
pair_6.set("geom1", "Part-2-1-collision")
pair_6.set("geom2", "floor")
pair_6.set("friction", "0.01 0.9 0.001 0.001 0.001")

pair_7 = ET.SubElement(contact, "pair")
pair_7.set("geom1", "Part-1-1-collision")
pair_7.set("geom2", "floor")
pair_7.set("friction", "1 10 3 10 10")

ballbot.add_custom_element_by_tag(name="contact", parent_tag="mujoco", element=contact)

Expand Down
7 changes: 4 additions & 3 deletions onshape_robotics_toolkit/robot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -364,21 +364,22 @@ def set_element_attributes(
self.mutated_elements[element_name] = attributes

def add_ground_plane(
self, root: ET.Element, size: int = 2, orientation: tuple[float, float, float] = (0, 0, 0)
self, root: ET.Element, size: int = 2, orientation: tuple[float, float, float] = (0, 0, 0), name: str = "floor"
) -> ET.Element:
"""
Add a ground plane to the root element with associated texture and material.
Args:
root: The root element to append the ground plane to (e.g. "asset", "worldbody")
size: Size of the ground plane (default: 2)
orientation: Euler angles for orientation (default: (0, 0, 0))
name: Name of the ground plane (default: "floor")
Returns:
ET.Element: The ground plane element
"""
# Create ground plane geom element
ground_geom = ET.Element(
"geom",
name=name,
type="plane",
pos=" ".join(map(str, self.ground_position)),
euler=" ".join(map(str, orientation)),
Expand All @@ -389,7 +390,7 @@ def add_ground_plane(
)

# Add to custom elements
self.add_custom_element_by_tag("ground", "worldbody", ground_geom)
self.add_custom_element_by_tag(name, "worldbody", ground_geom)

return ground_geom

Expand Down

0 comments on commit 22d3ca7

Please sign in to comment.