Unverified Commit 77a64984 authored by Michael Noseworthy's avatar Michael Noseworthy Committed by GitHub

Adds FORGE tasks for contact-rich manipulation with force sensing to IsaacLab (#2968)

# Description

This MR adds new tasks which extend the `Factory` tasks to include:
1. Force sensing: Add observations for force experienced by the
end-effector.
2. Excessive force penalty: Add an option to penalize the agent for
excessive contact forces.
3. Dynamics randomization: Randomize controller gains, asset properties
(friction, mass), and dead-zone.
4. Success prediction: Add an extra action that predicts task success.

The new tasks are: `Isaac-Forge-PegInsert-Direct-v0`,
`Isaac-Forge-GearMesh-Direct-v0`, and `Isaac-Forge-NutThread-Direct-v0`

## Type of change

- New feature (non-breaking change which adds functionality)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------
Signed-off-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
Co-authored-by: 's avatarOcti Zhang <zhengyuz@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
parent 5fb96c65
......@@ -360,5 +360,10 @@
"package": "referencing",
"license": "UNKNOWN",
"comment": "MIT"
},
{
"package": "regex",
"license": "UNKNOWN",
"comment": "Apache 2.0"
}
]
......@@ -245,6 +245,44 @@ We provide environments for both disassembly and assembly.
.. |assembly-link| replace:: `Isaac-AutoMate-Assembly-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env_cfg.py>`__
.. |disassembly-link| replace:: `Isaac-AutoMate-Disassembly-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env_cfg.py>`__
FORGE
~~~~~~~~
FORGE environments extend Factory environments with:
* Force sensing: Add observations for force experienced by the end-effector.
* Excessive force penalty: Add an option to penalize the agent for excessive contact forces.
* Dynamics randomization: Randomize controller gains, asset properties (friction, mass), and dead-zone.
* Success prediction: Add an extra action that predicts task success.
These tasks share the same task configurations and control options. You can switch between them by specifying the task name.
* |forge-peg-link|: Peg insertion with the Franka arm
* |forge-gear-link|: Gear meshing with the Franka arm
* |forge-nut-link|: Nut-Bolt fastening with the Franka arm
.. table::
:widths: 33 37 30
+--------------------+-------------------------+-----------------------------------------------------------------------------+
| World | Environment ID | Description |
+====================+=========================+=============================================================================+
| |forge-peg| | |forge-peg-link| | Insert peg into the socket with the Franka robot |
+--------------------+-------------------------+-----------------------------------------------------------------------------+
| |forge-gear| | |forge-gear-link| | Insert and mesh gear into the base with other gears, using the Franka robot |
+--------------------+-------------------------+-----------------------------------------------------------------------------+
| |forge-nut| | |forge-nut-link| | Thread the nut onto the first 2 threads of the bolt, using the Franka robot |
+--------------------+-------------------------+-----------------------------------------------------------------------------+
.. |forge-peg| image:: ../_static/tasks/factory/peg_insert.jpg
.. |forge-gear| image:: ../_static/tasks/factory/gear_mesh.jpg
.. |forge-nut| image:: ../_static/tasks/factory/nut_thread.jpg
.. |forge-peg-link| replace:: `Isaac-Forge-PegInsert-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env_cfg.py>`__
.. |forge-gear-link| replace:: `Isaac-Forge-GearMesh-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env_cfg.py>`__
.. |forge-nut-link| replace:: `Isaac-Forge-NutThread-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/forge/forge_env_cfg.py>`__
Locomotion
~~~~~~~~~~
......@@ -743,6 +781,18 @@ inferencing, including reading from an already trained checkpoint and disabling
-
- Direct
-
* - Isaac-Forge-GearMesh-Direct-v0
-
- Direct
- **rl_games** (PPO)
* - Isaac-Forge-NutThread-Direct-v0
-
- Direct
- **rl_games** (PPO)
* - Isaac-Forge-PegInsert-Direct-v0
-
- Direct
- **rl_games** (PPO)
* - Isaac-Franka-Cabinet-Direct-v0
-
- Direct
......
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.10.36"
version = "0.10.37"
# Description
title = "Isaac Lab Environments"
......
Changelog
---------
0.10.37 (2025-07-16)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added ``Isaac-Forge-PegInsert-Direct-v0``, ``Isaac-Forge-GearMesh-Direct-v0``,
and ``Isaac-Forge-NutThread-Direct-v0`` environments as direct RL envs. These
environments extend ``Isaac-Factory-*-v0`` with force sensing, an excessive force
penalty, dynamics randomization, and success prediction.
0.10.36 (2025-06-26)
~~~~~~~~~~~~~~~~~~~~
......
......@@ -31,6 +31,7 @@ def compute_dof_torque(
task_prop_gains,
task_deriv_gains,
device,
dead_zone_thresholds=None,
):
"""Compute Franka DOF torque to move fingertips towards target pose."""
# References:
......@@ -61,6 +62,15 @@ def compute_dof_torque(
)
task_wrench += task_wrench_motion
# Offset task_wrench motion by random amount to simulate unreliability at low forces.
# Check if absolute value is less than specified amount. If so, 0 out, otherwise, subtract.
if dead_zone_thresholds is not None:
task_wrench = torch.where(
task_wrench.abs() < dead_zone_thresholds,
torch.zeros_like(task_wrench),
task_wrench.sign() * (task_wrench.abs() - dead_zone_thresholds),
)
# Set tau = J^T * tau, i.e., map tau into joint space as desired
jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
dof_torque[:, 0:7] = (jacobian_T @ task_wrench.unsqueeze(-1)).squeeze(-1)
......@@ -135,7 +145,7 @@ def get_pose_error(
return pos_error, axis_angle_error
def _get_delta_dof_pos(delta_pose, ik_method, jacobian, device):
def get_delta_dof_pos(delta_pose, ik_method, jacobian, device):
"""Get delta Franka DOF position from delta pose using specified IK method."""
# References:
# 1) https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf
......
......@@ -163,6 +163,7 @@ class FactoryEnvCfg(DirectRLEnvCfg):
friction=0.0,
armature=0.0,
effort_limit_sim=87,
velocity_limit_sim=124.6,
),
"panda_arm2": ImplicitActuatorCfg(
joint_names_expr=["panda_joint[5-7]"],
......@@ -171,10 +172,12 @@ class FactoryEnvCfg(DirectRLEnvCfg):
friction=0.0,
armature=0.0,
effort_limit_sim=12,
velocity_limit_sim=149.5,
),
"panda_hand": ImplicitActuatorCfg(
joint_names_expr=["panda_finger_joint[1-2]"],
effort_limit_sim=40.0,
velocity_limit_sim=0.04,
stiffness=7500.0,
damping=173.0,
friction=0.1,
......
......@@ -67,7 +67,7 @@ class FactoryTask:
# Reward
ee_success_yaw: float = 0.0 # nut_thread task only.
action_penalty_scale: float = 0.0
action_penalty_ee_scale: float = 0.0
action_grad_penalty_scale: float = 0.0
# Reward function details can be found in Appendix B of https://arxiv.org/pdf/2408.04587.
# Multi-scale keypoints are used to capture different phases of the task.
......@@ -206,7 +206,6 @@ class GearMesh(FactoryTask):
name = "gear_mesh"
fixed_asset_cfg = GearBase()
held_asset_cfg = MediumGear()
target_gear = "gear_medium"
duration_s = 20.0
small_gear_usd = f"{ASSET_DIR}/factory_gear_small.usd"
......
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
import torch
import isaacsim.core.utils.torch as torch_utils
def get_keypoint_offsets(num_keypoints, device):
"""Get uniformly-spaced keypoints along a line of unit length, centered at 0."""
keypoint_offsets = torch.zeros((num_keypoints, 3), device=device)
keypoint_offsets[:, -1] = torch.linspace(0.0, 1.0, num_keypoints, device=device) - 0.5
return keypoint_offsets
def get_deriv_gains(prop_gains, rot_deriv_scale=1.0):
"""Set robot gains using critical damping."""
deriv_gains = 2 * torch.sqrt(prop_gains)
deriv_gains[:, 3:6] /= rot_deriv_scale
return deriv_gains
def wrap_yaw(angle):
"""Ensure yaw stays within range."""
return torch.where(angle > np.deg2rad(235), angle - 2 * np.pi, angle)
def set_friction(asset, value, num_envs):
"""Update material properties for a given asset."""
materials = asset.root_physx_view.get_material_properties()
materials[..., 0] = value # Static friction.
materials[..., 1] = value # Dynamic friction.
env_ids = torch.arange(num_envs, device="cpu")
asset.root_physx_view.set_material_properties(materials, env_ids)
def set_body_inertias(robot, num_envs):
"""Note: this is to account for the asset_options.armature parameter in IGE."""
inertias = robot.root_physx_view.get_inertias()
offset = torch.zeros_like(inertias)
offset[:, :, [0, 4, 8]] += 0.01
new_inertias = inertias + offset
robot.root_physx_view.set_inertias(new_inertias, torch.arange(num_envs))
def get_held_base_pos_local(task_name, fixed_asset_cfg, num_envs, device):
"""Get transform between asset default frame and geometric base frame."""
held_base_x_offset = 0.0
if task_name == "peg_insert":
held_base_z_offset = 0.0
elif task_name == "gear_mesh":
gear_base_offset = fixed_asset_cfg.medium_gear_base_offset
held_base_x_offset = gear_base_offset[0]
held_base_z_offset = gear_base_offset[2]
elif task_name == "nut_thread":
held_base_z_offset = fixed_asset_cfg.base_height
else:
raise NotImplementedError("Task not implemented")
held_base_pos_local = torch.tensor([0.0, 0.0, 0.0], device=device).repeat((num_envs, 1))
held_base_pos_local[:, 0] = held_base_x_offset
held_base_pos_local[:, 2] = held_base_z_offset
return held_base_pos_local
def get_held_base_pose(held_pos, held_quat, task_name, fixed_asset_cfg, num_envs, device):
"""Get current poses for keypoint and success computation."""
held_base_pos_local = get_held_base_pos_local(task_name, fixed_asset_cfg, num_envs, device)
held_base_quat_local = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).unsqueeze(0).repeat(num_envs, 1)
held_base_quat, held_base_pos = torch_utils.tf_combine(
held_quat, held_pos, held_base_quat_local, held_base_pos_local
)
return held_base_pos, held_base_quat
def get_target_held_base_pose(fixed_pos, fixed_quat, task_name, fixed_asset_cfg, num_envs, device):
"""Get target poses for keypoint and success computation."""
fixed_success_pos_local = torch.zeros((num_envs, 3), device=device)
if task_name == "peg_insert":
fixed_success_pos_local[:, 2] = 0.0
elif task_name == "gear_mesh":
gear_base_offset = fixed_asset_cfg.medium_gear_base_offset
fixed_success_pos_local[:, 0] = gear_base_offset[0]
fixed_success_pos_local[:, 2] = gear_base_offset[2]
elif task_name == "nut_thread":
head_height = fixed_asset_cfg.base_height
shank_length = fixed_asset_cfg.height
thread_pitch = fixed_asset_cfg.thread_pitch
fixed_success_pos_local[:, 2] = head_height + shank_length - thread_pitch * 1.5
else:
raise NotImplementedError("Task not implemented")
fixed_success_quat_local = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).unsqueeze(0).repeat(num_envs, 1)
target_held_base_quat, target_held_base_pos = torch_utils.tf_combine(
fixed_quat, fixed_pos, fixed_success_quat_local, fixed_success_pos_local
)
return target_held_base_pos, target_held_base_quat
def squashing_fn(x, a, b):
"""Compute bounded reward function."""
return 1 / (torch.exp(a * x) + b + torch.exp(-a * x))
def collapse_obs_dict(obs_dict, obs_order):
"""Stack observations in given order."""
obs_tensors = [obs_dict[obs_name] for obs_name in obs_order]
obs_tensors = torch.cat(obs_tensors, dim=-1)
return obs_tensors
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import gymnasium as gym
from . import agents
from .forge_env import ForgeEnv
from .forge_env_cfg import ForgeTaskGearMeshCfg, ForgeTaskNutThreadCfg, ForgeTaskPegInsertCfg
##
# Register Gym environments.
##
gym.register(
id="Isaac-Forge-PegInsert-Direct-v0",
entry_point="isaaclab_tasks.direct.forge:ForgeEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": ForgeTaskPegInsertCfg,
"rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml",
},
)
gym.register(
id="Isaac-Forge-GearMesh-Direct-v0",
entry_point="isaaclab_tasks.direct.forge:ForgeEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": ForgeTaskGearMeshCfg,
"rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml",
},
)
gym.register(
id="Isaac-Forge-NutThread-Direct-v0",
entry_point="isaaclab_tasks.direct.forge:ForgeEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": ForgeTaskNutThreadCfg,
"rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg_nut_thread.yaml",
},
)
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
params:
seed: 0
algo:
name: a2c_continuous
env:
clip_actions: 1.0
model:
name: continuous_a2c_logstd
network:
name: actor_critic
separate: False
space:
continuous:
mu_activation: None
sigma_activation: None
mu_init:
name: default
sigma_init:
name: const_initializer
val: 0
fixed_sigma: False
mlp:
units: [512, 128, 64]
activation: elu
d2rl: False
initializer:
name: default
regularizer:
name: None
rnn:
name: lstm
units: 1024
layers: 2
before_mlp: True
concat_input: True
layer_norm: True
load_checkpoint: False
load_path: ""
config:
name: Forge
device: cuda:0
full_experiment_name: test
env_name: rlgpu
multi_gpu: False
ppo: True
mixed_precision: True
normalize_input: True
normalize_value: True
value_bootstrap: True
num_actors: 128
reward_shaper:
scale_value: 1.0
normalize_advantage: True
gamma: 0.995
tau: 0.95
learning_rate: 1.0e-4
lr_schedule: adaptive
schedule_type: standard
kl_threshold: 0.008
score_to_win: 20000
max_epochs: 200
save_best_after: 10
save_frequency: 100
print_stats: True
grad_norm: 1.0
entropy_coef: 0.0
truncate_grads: True
e_clip: 0.2
horizon_length: 128
minibatch_size: 512 # batch size = num_envs * horizon_length; minibatch_size = batch_size / num_minibatches
mini_epochs: 4
critic_coef: 2
clip_value: True
seq_length: 128
bounds_loss_coef: 0.0001
central_value_config:
minibatch_size: 512
mini_epochs: 4
learning_rate: 1e-4
lr_schedule: adaptive
kl_threshold: 0.008
clip_value: True
normalize_input: True
truncate_grads: True
network:
name: actor_critic
central_value: True
mlp:
units: [512, 128, 64]
activation: elu
d2rl: False
initializer:
name: default
regularizer:
name: None
rnn:
name: lstm
units: 1024
layers: 2
before_mlp: True
concat_input: True
layer_norm: True
player:
deterministic: False
params:
seed: 0
algo:
name: a2c_continuous
env:
clip_actions: 1.0
model:
name: continuous_a2c_logstd
network:
name: actor_critic
separate: False
space:
continuous:
mu_activation: None
sigma_activation: None
mu_init:
name: default
sigma_init:
name: const_initializer
val: 0
fixed_sigma: False
mlp:
units: [512, 128, 64]
activation: elu
d2rl: False
initializer:
name: default
regularizer:
name: None
rnn:
name: lstm
units: 1024
layers: 2
before_mlp: True
concat_input: True
layer_norm: True
load_checkpoint: False
load_path: ""
config:
name: Forge
device: cuda:0
full_experiment_name: test
env_name: rlgpu
multi_gpu: False
ppo: True
mixed_precision: True
normalize_input: True
normalize_value: True
value_bootstrap: True
num_actors: 128
reward_shaper:
scale_value: 1.0
normalize_advantage: True
gamma: 0.995
tau: 0.95
learning_rate: 1.0e-4
lr_schedule: adaptive
schedule_type: standard
kl_threshold: 0.008
score_to_win: 20000
max_epochs: 200
save_best_after: 10
save_frequency: 100
print_stats: True
grad_norm: 1.0
entropy_coef: 0.0
truncate_grads: True
e_clip: 0.2
horizon_length: 256
minibatch_size: 512 # batch size = num_envs * horizon_length; minibatch_size = batch_size / num_minibatches
mini_epochs: 4
critic_coef: 2
clip_value: True
seq_length: 128
bounds_loss_coef: 0.0001
central_value_config:
minibatch_size: 512
mini_epochs: 4
learning_rate: 1e-4
lr_schedule: adaptive
kl_threshold: 0.008
clip_value: True
normalize_input: True
truncate_grads: True
network:
name: actor_critic
central_value: True
mlp:
units: [512, 128, 64]
activation: elu
d2rl: False
initializer:
name: default
regularizer:
name: None
rnn:
name: lstm
units: 1024
layers: 2
before_mlp: True
concat_input: True
layer_norm: True
player:
deterministic: False
This diff is collapsed.
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import isaaclab.envs.mdp as mdp
from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.utils import configclass
from isaaclab_tasks.direct.factory.factory_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, CtrlCfg, FactoryEnvCfg, ObsRandCfg
from .forge_events import randomize_dead_zone
from .forge_tasks_cfg import ForgeGearMesh, ForgeNutThread, ForgePegInsert, ForgeTask
OBS_DIM_CFG.update({"force_threshold": 1, "ft_force": 3})
STATE_DIM_CFG.update({"force_threshold": 1, "ft_force": 3})
@configclass
class ForgeCtrlCfg(CtrlCfg):
ema_factor_range = [0.025, 0.1]
default_task_prop_gains = [565.0, 565.0, 565.0, 28.0, 28.0, 28.0]
task_prop_gains_noise_level = [0.41, 0.41, 0.41, 0.41, 0.41, 0.41]
pos_threshold_noise_level = [0.25, 0.25, 0.25]
rot_threshold_noise_level = [0.29, 0.29, 0.29]
default_dead_zone = [5.0, 5.0, 5.0, 1.0, 1.0, 1.0]
@configclass
class ForgeObsRandCfg(ObsRandCfg):
fingertip_pos = 0.00025
fingertip_rot_deg = 0.1
ft_force = 1.0
@configclass
class EventCfg:
object_scale_mass = EventTerm(
func=mdp.randomize_rigid_body_mass,
mode="reset",
params={
"asset_cfg": SceneEntityCfg("held_asset"),
"mass_distribution_params": (-0.005, 0.005),
"operation": "add",
"distribution": "uniform",
},
)
held_physics_material = EventTerm(
func=mdp.randomize_rigid_body_material,
mode="startup",
params={
"asset_cfg": SceneEntityCfg("held_asset"),
"static_friction_range": (0.75, 0.75),
"dynamic_friction_range": (0.75, 0.75),
"restitution_range": (0.0, 0.0),
"num_buckets": 1,
},
)
fixed_physics_material = EventTerm(
func=mdp.randomize_rigid_body_material,
mode="startup",
params={
"asset_cfg": SceneEntityCfg("fixed_asset"),
"static_friction_range": (0.25, 1.25), # TODO: Set these values based on asset type.
"dynamic_friction_range": (0.25, 0.25),
"restitution_range": (0.0, 0.0),
"num_buckets": 128,
},
)
robot_physics_material = EventTerm(
func=mdp.randomize_rigid_body_material,
mode="startup",
params={
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
"static_friction_range": (0.75, 0.75),
"dynamic_friction_range": (0.75, 0.75),
"restitution_range": (0.0, 0.0),
"num_buckets": 1,
},
)
dead_zone_thresholds = EventTerm(
func=randomize_dead_zone, mode="interval", interval_range_s=(2.0, 2.0) # (0.25, 0.25)
)
@configclass
class ForgeEnvCfg(FactoryEnvCfg):
action_space: int = 7
obs_rand: ForgeObsRandCfg = ForgeObsRandCfg()
ctrl: ForgeCtrlCfg = ForgeCtrlCfg()
task: ForgeTask = ForgeTask()
events: EventCfg = EventCfg()
ft_smoothing_factor: float = 0.25
obs_order: list = [
"fingertip_pos_rel_fixed",
"fingertip_quat",
"ee_linvel",
"ee_angvel",
"ft_force",
"force_threshold",
]
state_order: list = [
"fingertip_pos",
"fingertip_quat",
"ee_linvel",
"ee_angvel",
"joint_pos",
"held_pos",
"held_pos_rel_fixed",
"held_quat",
"fixed_pos",
"fixed_quat",
"task_prop_gains",
"ema_factor",
"ft_force",
"pos_threshold",
"rot_threshold",
"force_threshold",
]
@configclass
class ForgeTaskPegInsertCfg(ForgeEnvCfg):
task_name = "peg_insert"
task = ForgePegInsert()
episode_length_s = 10.0
@configclass
class ForgeTaskGearMeshCfg(ForgeEnvCfg):
task_name = "gear_mesh"
task = ForgeGearMesh()
episode_length_s = 20.0
@configclass
class ForgeTaskNutThreadCfg(ForgeEnvCfg):
task_name = "nut_thread"
task = ForgeNutThread()
episode_length_s = 30.0
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import torch
from isaaclab.envs import DirectRLEnv
def randomize_dead_zone(env: DirectRLEnv, env_ids: torch.Tensor | None):
env.dead_zone_thresholds = (
torch.rand((env.num_envs, 6), dtype=torch.float32, device=env.device) * env.default_dead_zone
)
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from isaaclab.utils import configclass
from isaaclab_tasks.direct.factory.factory_tasks_cfg import FactoryTask, GearMesh, NutThread, PegInsert
@configclass
class ForgeTask(FactoryTask):
action_penalty_ee_scale: float = 0.0
action_penalty_asset_scale: float = 0.001
action_grad_penalty_scale: float = 0.1
contact_penalty_scale: float = 0.05
delay_until_ratio: float = 0.25
contact_penalty_threshold_range = [5.0, 10.0]
@configclass
class ForgePegInsert(PegInsert, ForgeTask):
contact_penalty_scale: float = 0.2
@configclass
class ForgeGearMesh(GearMesh, ForgeTask):
contact_penalty_scale: float = 0.05
@configclass
class ForgeNutThread(NutThread, ForgeTask):
contact_penalty_scale: float = 0.05
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import torch
import isaacsim.core.utils.torch as torch_utils
def get_random_prop_gains(default_values, noise_levels, num_envs, device):
"""Helper function to randomize controller gains."""
c_param_noise = torch.rand((num_envs, default_values.shape[1]), dtype=torch.float32, device=device)
c_param_noise = c_param_noise @ torch.diag(torch.tensor(noise_levels, dtype=torch.float32, device=device))
c_param_multiplier = 1.0 + c_param_noise
decrease_param_flag = torch.rand((num_envs, default_values.shape[1]), dtype=torch.float32, device=device) > 0.5
c_param_multiplier = torch.where(decrease_param_flag, 1.0 / c_param_multiplier, c_param_multiplier)
prop_gains = default_values * c_param_multiplier
return prop_gains
def change_FT_frame(source_F, source_T, source_frame, target_frame):
"""Convert force/torque reading from source to target frame."""
# Modern Robotics eq. 3.95
source_frame_inv = torch_utils.tf_inverse(source_frame[0], source_frame[1])
target_T_source_quat, target_T_source_pos = torch_utils.tf_combine(
source_frame_inv[0], source_frame_inv[1], target_frame[0], target_frame[1]
)
target_F = torch_utils.quat_apply(target_T_source_quat, source_F)
target_T = torch_utils.quat_apply(
target_T_source_quat, (source_T + torch.cross(target_T_source_pos, source_F, dim=-1))
)
return target_F, target_T
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment