Skip to content

K
KincoActuatorIsaacLab
Unverified Commit cf7a65f3 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub
Browse files
Options

Fixes the inverse kinematics example for Franka robot (#319) 

# Description

This MR makes sure the IK example works properly again. It adds the IK
action space to the lift environment. Additionally, it fixes the state
machine for the lift environment.

## Type of change

- Bug fix (non-breaking change which fixes an issue)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] 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
parent 818ff410
Expand all Show whitespace changes
Inline Side-by-side
Showing with 646 additions and 380 deletions
docs/source/setup/sample.rst
View file @ cf7a65f3
......@@ -73,7 +73,7 @@ allows efficient execution for large number of environments using CUDA kernels.
.. code:: bash
./orbit.sh -p source/standalone/environments/state_machine/play_lift.py --num_envs 32
./orbit.sh -p source/standalone/environments/state_machine/lift_cube_sm.py --num_envs 32
Teleoperation
......@@ -88,7 +88,7 @@ To play inverse kinematics (IK) control with a keyboard device:
.. code:: bash
./orbit.sh -p source/standalone/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Franka-v0 --num_envs 1 --cpu --device keyboard
./orbit.sh -p source/standalone/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --num_envs 1 --device keyboard
The script prints the teleoperation events configured. For keyboard,
these are as follows:
......@@ -116,14 +116,14 @@ data in
format.
1. Collect demonstrations with teleoperation for the environment
``Isaac-Lift-Franka-v0``:
``Isaac-Lift-Cube-Franka-IK-Rel-v0``:
.. code:: bash
# step a: collect data with keyboard
./orbit.sh -p source/standalone/workflows/robomimic/collect_demonstrations.py --task Isaac-Lift-Franka-v0 --num_envs 1 --num_demos 10 --device keyboard
./orbit.sh -p source/standalone/workflows/robomimic/collect_demonstrations.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --num_envs 1 --num_demos 10 --device keyboard
# step b: inspect the collected dataset
./orbit.sh -p source/standalone/workflows/robomimic/tools/inspect_demonstrations.py logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5
./orbit.sh -p source/standalone/workflows/robomimic/tools/inspect_demonstrations.py logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5
2. Split the dataset into train and validation set:
......@@ -132,20 +132,20 @@ format.
# install python module (for robomimic)
./orbit.sh -e robomimic
# split data
./orbit.sh -p source/standalone//workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5 --ratio 0.2
./orbit.sh -p source/standalone//workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5 --ratio 0.2
3. Train a BC agent for ``Isaac-Lift-Franka-v0`` with
3. Train a BC agent for ``Isaac-Lift-Cube-Franka-IK-Rel-v0`` with
`Robomimic <https://robomimic.github.io/>`__:
.. code:: bash
./orbit.sh -p source/standalone/workflows/robomimic/train.py --task Isaac-Lift-Franka-v0 --algo bc --dataset logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5
./orbit.sh -p source/standalone/workflows/robomimic/train.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --algo bc --dataset logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5
4. Play the learned model to visualize results:
.. code:: bash
./orbit.sh -p source/standalone//workflows/robomimic/play.py --task Isaac-Lift-Franka-v0 --checkpoint /PATH/TO/model.pth
./orbit.sh -p source/standalone//workflows/robomimic/play.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --checkpoint /PATH/TO/model.pth
Reinforcement Learning
~~~~~~~~~~~~~~~~~~~~~~
......
docs/source/tutorials/05_controllers/ik_controller.rst deleted 100644 → 0
View file @ 818ff410
Using a task-space controller
=============================
In the previous tutorials, we learned how to create a scene and control a robotic arm using a
joint-space controller. In this tutorial, we will learn how to use a task-space controller to
control the robot. More specifically, we will use the :class:`DifferentialInverseKinematics` class
to track a desired end-effector pose command.
The Code
~~~~~~~~
The tutorial corresponds to the ``play_ik_control.py`` script in the ``orbit/source/standalone/demo`` directory.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:emphasize-lines: 44-47,130-138,147,201-217
:linenos:
The Code Explained
~~~~~~~~~~~~~~~~~~
While using any task-space controller, it is important to ensure that the provided
quantities are in the correct frames. When parallelizing environment instances using the cloner,
each environment itself can be thought of having its own local frame. However, from the way
the physics engine is implemented in Isaac Sim, all environments exist on the same stage and
thus there is a unique global frame for the entire simulation. In summary, there are the three
main frames that are used in Orbit:
- The simulation world frame (denoted as ``w``), which is the frame of the entire simulation.
- The local environment frame (denoted as ``e``), which is the frame of the local environment.
- The robot's base frame (denoted as ``b``), which is the frame of the robot's base link.
In the current scenario, where the robot is mounted on the table, the base frame of the robot coincides with
the local environment frame. However, this is not always the case. For example, in a scenario where the robot
is a floating-base system. The location of the environment frames are obtained from the
:attr:`envs_positions` value returned by the cloner.
Creating an IK controller
-------------------------
Computing the inverse kinematics (IK) of a robot is a common task in robotics.
The :class:`DifferentialInverseKinematics` class computes the desired joint positions
for a robot to reach a desired end-effector pose. The included implementation performs
the computation in a batched format and is optimized for speed.
Since in many robots the end-effector is not a rigid body, the simulator does not provide
the pose and Jacobian of the end-effector directly. Instead, the obtained Jacobian
is that of the parent body and not the end-effector. Thus, the IK controller takes in
as input the end-effector offset from the parent frame. This offset is typically specified
in the robot's configuration instance and thus is obtained from there.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 130-138
:linenos:
:lineno-start: 130
Computing robot command
-----------------------
The IK controller separates the operation of setting the desired command and
computing the desired joint positions. This is done to allow for the user to
run the IK controller at a different frequency than the robot's control frequency.
The :attr:`set_command` method takes in the desired end-effector pose as a single
batched array. The first three columns correspond to the desired position and the
last four columns correspond to the desired quaternion orientation in ``(w, x, y, z)``
order.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 201-202
:linenos:
:lineno-start: 201
We can then compute the desired joint positions using the :attr:`compute` method.
The method takes in the current end-effector position, orientation, Jacobian, and
current joint positions. We read the Jacobian matrix from the robot's data, which uses
its value computed from the physics engine.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 203-209
:linenos:
:lineno-start: 203
While the IK controller returns the desired joint positions, we need to convert
them to the robot's action space. This is done by subtracting joint positions
offsets from the desired joint positions. The joint offsets are obtained from the
robot's data which is a constant value obtained from the robot's configuration.
For more details, we suggest reading the :doc:`/source/api/orbit.actuators.group` tutorial.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 210-215
:linenos:
:lineno-start: 210
These actions can then be applied on the robot, as done in the previous tutorials.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 216-219
:linenos:
:lineno-start: 216
Using markers for displaying frames
-----------------------------------
We will use the :class:`StaticMarker` class to display the current and desired end-effector poses.
The marker class takes as input the associated prim name, the number of markers to display, the
USD file corresponding to the marker, and the scale of the marker. By default, it uses a frame marker
to display the pose.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 81-83
:linenos:
:lineno-start: 81
We can then set the pose of the marker using the :attr:`set_world_poses` method.
It is important to ensure that the set poses are in the simulation world frame and not the
local environment frame.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
:language: python
:lines: 223-229
:linenos:
:lineno-start: 223
The Code Execution
~~~~~~~~~~~~~~~~~~
Now that we have gone through the code, let's run the script and see the result:
.. code-block:: bash
./orbit.sh -p source/standalone/tutorials/05_controllers/ik_control.py --robot franka_panda --num_envs 128
The script will start a simulation with 128 robots. The robots will be controlled using a task-space controller.
The current and desired end-effector poses should be displayed using frame markers. When the robot reaches the
desired pose, the command should cycle through to the next pose specified in the script.
To stop the simulation, you can either close the window, or press the ``STOP`` button in the UI, or
press ``Ctrl+C`` in the terminal.
docs/source/tutorials/05_controllers/index.rst
View file @ cf7a65f3
......@@ -8,4 +8,4 @@ tutorials show you how to use motion generators to control the robots at the tas
:maxdepth: 1
:titlesonly:
ik_controller
run_diff_ik
docs/source/tutorials/05_controllers/run_diff_ik.rst 0 → 100644
View file @ cf7a65f3
Using a task-space controller
=============================
.. currentmodule:: omni.isaac.orbit
In the previous tutorials, we have joint-space controllers to control the robot. However, in many
cases, it is more intuitive to control the robot using a task-space controller. For example, if we
want to teleoperate the robot, it is easier to specify the desired end-effector pose rather than
the desired joint positions.
In this tutorial, we will learn how to use a task-space controller to control the robot.
We will use the :class:`controllers.DifferentialIKController` class to track a desired
end-effector pose command.
The Code
~~~~~~~~
The tutorial corresponds to the ``run_diff_ik.py`` script in the ``orbit/source/standalone/demo`` directory.
.. dropdown:: Code for run_diff_ik.py
:icon: code
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
:language: python
:emphasize-lines: 99-101, 122-137, 156-158, 162-172
:linenos:
The Code Explained
~~~~~~~~~~~~~~~~~~
While using any task-space controller, it is important to ensure that the provided
quantities are in the correct frames. When parallelizing environment instances, they are
all existing in the same unique simulation world frame. However, typically, we want each
environment itself to have its own local frame. This is accessible through the
:attr:`scene.InteractiveScene.env_origins` attribute.
In our APIs, we use the following notation for frames:
- The simulation world frame (denoted as ``w``), which is the frame of the entire simulation.
- The local environment frame (denoted as ``e``), which is the frame of the local environment.
- The robot's base frame (denoted as ``b``), which is the frame of the robot's base link.
Since the asset instances are not "aware" of the local environment frame, they return
their states in the simulation world frame. Thus, we need to convert the obtained
quantities to the local environment frame. This is done by subtracting the local environment
origin from the obtained quantities.
Creating an IK controller
-------------------------
The :class:`~controllers.DifferentialIKController` class computes the desired joint
positions for a robot to reach a desired end-effector pose. The included implementation
performs the computation in a batched format and uses PyTorch operations. It supports
different types of inverse kinematics solvers, including the damped least-squares method
and the pseudo-inverse method. These solvers can be specified using the
:attr:`~controllers.DifferentialIKControllerCfg.ik_method` argument.
Additionally, the controller can handle commands as both relative and absolute poses.
In this tutorial, we will use the damped least-squares method to compute the desired
joint positions. Additionally, since we want to track desired end-effector poses, we
will use the absolute pose command mode.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
:language: python
:lines: 99-101
:linenos:
:lineno-start: 99
Obtaining the robot's joint and body indices
--------------------------------------------
The IK controller implementation is a computation-only class. Thus, it expects the
user to provide the necessary information about the robot. This includes the robot's
joint positions, current end-effector pose, and the Jacobian matrix.
While the attribute :attr:`assets.ArticulationData.joint_pos` provides the joint positions,
we only want the joint positions of the robot's arm, and not the gripper. Similarly, while
the attribute :attr:`assets.ArticulationData.body_state_w` provides the state of all the
robot's bodies, we only want the state of the robot's end-effector. Thus, we need to
index into these arrays to obtain the desired quantities.
For this, the articulation class provides the methods :meth:`~assets.Articulation.find_joints`
and :meth:`~assets.Articulation.find_bodies`. These methods take in the names of the joints
and bodies and return their corresponding indices.
While you may directly use these methods to obtain the indices, we recommend using the
:attr:`~managers.SceneEntityCfg` class to resolve the indices. This class is used in various
places in the APIs to extract certain information from a scene entity. Internally, it
calls the above methods to obtain the indices. However, it also performs some additional
checks to ensure that the provided names are valid. Thus, it is a safer option to use
this class.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
:language: python
:lines: 122-137
:linenos:
:lineno-start: 122
Computing robot command
-----------------------
The IK controller separates the operation of setting the desired command and
computing the desired joint positions. This is done to allow for the user to
run the IK controller at a different frequency than the robot's control frequency.
The :meth:`~controllers.DifferentialIKController.set_command` method takes in
the desired end-effector pose as a single batched array. The pose is specified in
the robot's base frame.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
:language: python
:lines: 156-158
:linenos:
:lineno-start: 156
We can then compute the desired joint positions using the
:meth:`~controllers.DifferentialIKController.compute` method.
The method takes in the current end-effector pose (in base frame), Jacobian, and
current joint positions. We read the Jacobian matrix from the robot's data, which uses
its value computed from the physics engine.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
:language: python
:lines: 162-172
:linenos:
:lineno-start: 162
The computed joint position targets can then be applied on the robot, as done in the
previous tutorials.
.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
:language: python
:lines: 174-176
:linenos:
:lineno-start: 174
The Code Execution
~~~~~~~~~~~~~~~~~~
Now that we have gone through the code, let's run the script and see the result:
.. code-block:: bash
./orbit.sh -p source/standalone/tutorials/05_controllers/run_diff_ik.py --robot franka_panda --num_envs 128
The script will start a simulation with 128 robots. The robots will be controlled using the IK controller.
The current and desired end-effector poses should be displayed using frame markers. When the robot reaches
the desired pose, the command should cycle through to the next pose specified in the script.
To stop the simulation, you can either close the window, or press the ``STOP`` button in the UI, or
press ``Ctrl+C`` in the terminal.
source/extensions/omni.isaac.orbit/config/extension.toml
View file @ cf7a65f3
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.10.7"
version = "0.10.8"
# Description
title = "ORBIT framework for Robot Learning"
......
source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
View file @ cf7a65f3
Changelog
---------
0.10.8 (2023-12-20)
~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* Fixed the :class:`omni.isaac.orbit.envs.mdp.actions.DifferentialInverseKinematicsAction` class
to account for the offset pose of the end-effector.
0.10.7 (2023-12-19)
~~~~~~~~~~~~~~~~~~~
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/assets/config/franka.py
View file @ cf7a65f3
......@@ -7,7 +7,8 @@
The following configurations are available:
* :obj:`FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG`: Franka Emika Panda robot with Panda hand
* :obj:`FRANKA_PANDA_CFG`: Franka Emika Panda robot with Panda hand
* :obj:`FRANKA_PANDA_HIGH_PD_CFG`: Franka Emika Panda robot with Panda hand with stiffer PD control
Reference: https://github.com/frankaemika/franka_ros
"""
......@@ -22,7 +23,7 @@ from ..articulation import ArticulationCfg
# Configuration
##
FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = ArticulationCfg(
FRANKA_PANDA_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd",
activate_contact_sensors=False,
......@@ -51,14 +52,14 @@ FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = ArticulationCfg(
"panda_shoulder": ImplicitActuatorCfg(
joint_names_expr=["panda_joint[1-4]"],
effort_limit=87.0,
velocity_limit=100.0,
velocity_limit=2.175,
stiffness=80.0,
damping=4.0,
),
"panda_forearm": ImplicitActuatorCfg(
joint_names_expr=["panda_joint[5-7]"],
effort_limit=87.0,
velocity_limit=100.0,
effort_limit=12.0,
velocity_limit=2.61,
stiffness=80.0,
damping=4.0,
),
......@@ -72,4 +73,16 @@ FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = ArticulationCfg(
},
soft_joint_pos_limit_factor=1.0,
)
"""Configuration of Franka Emika Panda robot with Panda hand."""
"""Configuration of Franka Emika Panda robot."""
FRANKA_PANDA_HIGH_PD_CFG = FRANKA_PANDA_CFG.copy()
FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True
FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].stiffness = 400.0
FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].damping = 80.0
FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].stiffness = 400.0
FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].damping = 80.0
"""Configuration of Franka Emika Panda robot with stiffer PD control.
This configuration is useful for task-space control using differential IK.
"""
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/differential_ik_cfg.py
View file @ cf7a65f3
......@@ -49,7 +49,7 @@ class DifferentialIKControllerCfg:
- Jacobian transpose ("trans"):
- "k_val": Scaling of computed delta-joint positions (default: 1.0).
- Damped Moore-Penrose pseudo-inverse ("dls"):
- "lambda_val": Damping coefficient (default: 0.1).
- "lambda_val": Damping coefficient (default: 0.01).
"""
def __post_init__(self):
......@@ -63,7 +63,7 @@ class DifferentialIKControllerCfg:
"pinv": {"k_val": 1.0},
"svd": {"k_val": 1.0, "min_singular_value": 1e-5},
"trans": {"k_val": 1.0},
"dls": {"lambda_val": 0.1},
"dls": {"lambda_val": 0.01},
}
# update parameters for IK-method if not provided
ik_params = default_ik_params[self.ik_method].copy()
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/actions/actions_cfg.py
View file @ cf7a65f3
......@@ -157,12 +157,29 @@ class DifferentialInverseKinematicsActionCfg(ActionTermCfg):
See :class:`DifferentialInverseKinematicsAction` for more details.
"""
@configclass
class OffsetCfg:
"""The offset pose from parent frame to child frame.
On many robots, end-effector frames are fictitious frames that do not have a corresponding
rigid body. In such cases, it is easier to define this transform w.r.t. their parent rigid body.
For instance, for the Franka Emika arm, the end-effector is defined at an offset to the the
"panda_hand" frame.
"""
pos: tuple[float, float, float] = (0.0, 0.0, 0.0)
"""Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0)."""
rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
"""Quaternion rotation ``(w, x, y, z)`` w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0)."""
class_type: type[ActionTerm] = task_space_actions.DifferentialInverseKinematicsAction
joint_names: list[str] = MISSING
"""List of joint names or regex expressions that the action will be mapped to."""
body_name: str = MISSING
"""Name of the body or frame for which IK is performed."""
body_offset: OffsetCfg | None = None
"""Offset of target frame w.r.t. to the body frame. Defaults to None, in which case no offset is applied."""
scale: float | tuple[float, ...] = 1.0
"""Scale factor for the action. Defaults to 1.0."""
controller: DifferentialIKControllerCfg = MISSING
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/actions/task_space_actions.py
View file @ cf7a65f3
......@@ -10,10 +10,10 @@ from typing import TYPE_CHECKING
import carb
import omni.isaac.orbit.utils.math as math_utils
from omni.isaac.orbit.assets.articulation import Articulation
from omni.isaac.orbit.controllers.differential_ik import DifferentialIKController
from omni.isaac.orbit.managers.action_manager import ActionTerm
from omni.isaac.orbit.utils.math import subtract_frame_transforms
if TYPE_CHECKING:
from omni.isaac.orbit.envs import BaseEnv
......@@ -79,7 +79,9 @@ class DifferentialInverseKinematicsAction(ActionTerm):
self._joint_ids = slice(None)
# create the differential IK controller
self._controller = DifferentialIKController(cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device)
self._ik_controller = DifferentialIKController(
cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device
)
# create tensors for raw and processed actions
self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
......@@ -89,13 +91,20 @@ class DifferentialInverseKinematicsAction(ActionTerm):
self._scale = torch.zeros((self.num_envs, self.action_dim), device=self.device)
self._scale[:] = torch.tensor(self.cfg.scale, device=self.device)
# convert the fixed offsets to torch tensors of batched shape
if self.cfg.body_offset is not None:
self._offset_pos = torch.tensor(self.cfg.body_offset.pos, device=self.device).repeat(self.num_envs, 1)
self._offset_rot = torch.tensor(self.cfg.body_offset.rot, device=self.device).repeat(self.num_envs, 1)
else:
self._offset_pos, self._offset_rot = None, None
"""
Properties.
"""
@property
def action_dim(self) -> int:
return self._controller.action_dim
return self._ik_controller.action_dim
@property
def raw_actions(self) -> torch.Tensor:
......@@ -114,17 +123,20 @@ class DifferentialInverseKinematicsAction(ActionTerm):
self._raw_actions[:] = actions
self._processed_actions[:] = self.raw_actions * self._scale
# obtain quantities from simulation
ee_pos_curr, ee_quat_curr = self._compute_body_pose()
ee_pos_curr, ee_quat_curr = self._compute_frame_pose()
# set command into controller
self._ik_controller.set_command(self._processed_actions, ee_pos_curr, ee_quat_curr)
def apply_actions(self):
# obtain quantities from simulation
jacobian = self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._joint_ids]
ee_pos_curr, ee_quat_curr = self._compute_body_pose()
ee_pos_curr, ee_quat_curr = self._compute_frame_pose()
joint_pos = self._asset.data.joint_pos[:, self._joint_ids]
# compute the delta in joint-space
if ee_quat_curr.norm() != 0:
jacobian = self._compute_frame_jacobian()
joint_pos_des = self._ik_controller.compute(ee_pos_curr, ee_quat_curr, jacobian, joint_pos)
else:
joint_pos_des = joint_pos.clone()
# set the joint position command
self._asset.set_joint_position_target(joint_pos_des, self._joint_ids)
......@@ -132,8 +144,8 @@ class DifferentialInverseKinematicsAction(ActionTerm):
Helper functions.
"""
def _compute_body_pose(self) -> tuple[torch.Tensor, torch.Tensor]:
"""Computes the pose of the body in the root frame.
def _compute_frame_pose(self) -> tuple[torch.Tensor, torch.Tensor]:
"""Computes the pose of the target frame in the root frame.
Returns:
A tuple of the body's position and orientation in the root frame.
......@@ -142,4 +154,35 @@ class DifferentialInverseKinematicsAction(ActionTerm):
ee_pose_w = self._asset.data.body_state_w[:, self._body_idx, :7]
root_pose_w = self._asset.data.root_state_w[:, :7]
# compute the pose of the body in the root frame
return subtract_frame_transforms(root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
ee_pose_b, ee_quat_b = math_utils.subtract_frame_transforms(
root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
)
# account for the offset
if self.cfg.body_offset is not None:
ee_pose_b, ee_quat_b = math_utils.combine_frame_transforms(
ee_pose_b, ee_quat_b, self._offset_pos, self._offset_rot
)
return ee_pose_b, ee_quat_b
def _compute_frame_jacobian(self):
"""Computes the geometric Jacobian of the target frame in the root frame.
This function accounts for the target frame offset and applies the necessary transformations to obtain
the right Jacobian from the parent body Jacobian.
"""
# read the parent jacobian
jacobian = self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._joint_ids]
# account for the offset
if self.cfg.body_offset is not None:
# Modify the jacobian to account for the offset
# -- translational part
# v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee
# = (v_J_ee + w_J_ee x r_link_ee ) * q
# = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q
jacobian[:, 0:3, :] += torch.bmm(-math_utils.skew_symmetric_matrix(self._offset_pos), jacobian[:, 3:, :])
# -- rotational part
# w_link = R_link_ee @ w_ee
jacobian[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), jacobian[:, 3:, :])
return jacobian
source/extensions/omni.isaac.orbit/omni/isaac/orbit/managers/scene_entity_cfg.py
View file @ cf7a65f3
......@@ -35,13 +35,13 @@ class SceneEntityCfg:
The names can be either joint names or a regular expression matching the joint names.
These are converted to joint indices on initialization of the manager and passed to the term
function as a list of joint indices under :attr:`dof_ids`.
function as a list of joint indices under :attr:`joint_ids`.
"""
joint_ids: list[int] | None = None
"""The indices of the joints from the asset required by the term. Defaults to None.
If ``joint_names`` is specified, this is filled in automatically on initialization of the
If :attr:`joint_names` is specified, this is filled in automatically on initialization of the
manager.
"""
......@@ -57,7 +57,7 @@ class SceneEntityCfg:
body_ids: list[int] | None = None
"""The indices of the bodies from the asset required by the term. Defaults to None.
If ``body_names`` is specified, this is filled in automatically on initialization of the
If :attr:`body_names` is specified, this is filled in automatically on initialization of the
manager.
"""
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/math.py
View file @ cf7a65f3
......@@ -659,6 +659,37 @@ def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
return torch.norm(axis_angle_from_quat(quat_diff), dim=1)
@torch.jit.script
def skew_symmetric_matrix(vec: torch.Tensor) -> torch.Tensor:
"""Computes the skew-symmetric matrix of a vector.
Args:
vec: The input vector. Shape is (3,) or (N, 3).
Returns:
The skew-symmetric matrix. Shape is (1, 3, 3) or (N, 3, 3).
Raises:
ValueError: If input tensor is not of shape (..., 3).
"""
# check input is correct
if vec.shape[-1] != 3:
raise ValueError(f"Expected input vector shape mismatch: {vec.shape} != (..., 3).")
# unsqueeze the last dimension
if vec.ndim == 1:
vec = vec.unsqueeze(0)
# create a skew-symmetric matrix
skew_sym_mat = torch.zeros(vec.shape[0], 3, 3, device=vec.device, dtype=vec.dtype)
skew_sym_mat[:, 0, 1] = -vec[:, 2]
skew_sym_mat[:, 0, 2] = vec[:, 1]
skew_sym_mat[:, 1, 2] = -vec[:, 0]
skew_sym_mat[:, 1, 0] = vec[:, 2]
skew_sym_mat[:, 2, 0] = -vec[:, 1]
skew_sym_mat[:, 2, 1] = vec[:, 0]
return skew_sym_mat
"""
Transformations
"""
......
source/extensions/omni.isaac.orbit/test/assets/test_articulation.py
View file @ cf7a65f3
......@@ -30,7 +30,7 @@ import omni.isaac.orbit.sim as sim_utils
import omni.isaac.orbit.utils.string as string_utils
from omni.isaac.orbit.actuators import ImplicitActuatorCfg
from omni.isaac.orbit.assets import Articulation, ArticulationCfg
from omni.isaac.orbit.assets.config import ANYMAL_C_CFG, FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
from omni.isaac.orbit.assets.config import ANYMAL_C_CFG, FRANKA_PANDA_CFG
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
......@@ -120,7 +120,7 @@ class TestArticulation(unittest.TestCase):
def test_initialization_fixed_base(self):
"""Test initialization for fixed base."""
# Create articulation
robot = Articulation(cfg=FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/Robot"))
robot = Articulation(cfg=FRANKA_PANDA_CFG.replace(prim_path="/World/Robot"))
# Check that boundedness of articulation is correct
self.assertEqual(ctypes.c_long.from_address(id(robot)).value, 1)
......
source/extensions/omni.isaac.orbit/test/controllers/test_differential_ik.py
View file @ cf7a65f3
......@@ -25,7 +25,7 @@ from omni.isaac.cloner import GridCloner
import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.assets import Articulation
from omni.isaac.orbit.assets.config import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG, UR10_CFG
from omni.isaac.orbit.assets.config import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG
from omni.isaac.orbit.controllers import DifferentialIKController, DifferentialIKControllerCfg
from omni.isaac.orbit.utils.math import compute_pose_error, subtract_frame_transforms
......@@ -42,6 +42,8 @@ class TestDifferentialIKController(unittest.TestCase):
# Load kit helper
sim_cfg = sim_utils.SimulationCfg(dt=0.01)
self.sim = sim_utils.SimulationContext(sim_cfg)
# TODO: Remove this once we have a better way to handle this.
self.sim._app_control_on_stop_handle = None
# Create a ground plane
cfg = sim_utils.GroundPlaneCfg()
......@@ -83,8 +85,7 @@ class TestDifferentialIKController(unittest.TestCase):
def test_franka_ik_pose_abs(self):
"""Test IK controller for Franka arm with Franka hand."""
# Create robot instance
robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/envs/env_.*/Robot")
robot_cfg.spawn.rigid_props.disable_gravity = True
robot_cfg = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="/World/envs/env_.*/Robot")
robot = Articulation(cfg=robot_cfg)
# Create IK controller
......
source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/__init__.py
View file @ cf7a65f3
......@@ -6,16 +6,21 @@
import gymnasium as gym
from . import agents, env_cfg
from . import agents, ik_abs_env_cfg, ik_rel_env_cfg, joint_pos_env_cfg
##
# Register Gym environments.
##
##
# Joint Position Control
##
gym.register(
id="Isaac-Lift-Cube-Franka-v0",
entry_point="omni.isaac.orbit.envs:RLTaskEnv",
kwargs={
"env_cfg_entry_point": env_cfg.FrankaCubeLiftEnvCfg,
"env_cfg_entry_point": joint_pos_env_cfg.FrankaCubeLiftEnvCfg,
"rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
},
disable_env_checker=True,
......@@ -25,7 +30,55 @@ gym.register(
id="Isaac-Lift-Cube-Franka-Play-v0",
entry_point="omni.isaac.orbit.envs:RLTaskEnv",
kwargs={
"env_cfg_entry_point": env_cfg.FrankaCubeLiftEnvCfg_PLAY,
"env_cfg_entry_point": joint_pos_env_cfg.FrankaCubeLiftEnvCfg_PLAY,
"rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
},
disable_env_checker=True,
)
##
# Inverse Kinematics - Absolute Pose Control
##
gym.register(
id="Isaac-Lift-Cube-Franka-IK-Abs-v0",
entry_point="omni.isaac.orbit.envs:RLTaskEnv",
kwargs={
"env_cfg_entry_point": ik_abs_env_cfg.FrankaCubeLiftEnvCfg,
"rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
},
disable_env_checker=True,
)
gym.register(
id="Isaac-Lift-Cube-Franka-IK-Abs-Play-v0",
entry_point="omni.isaac.orbit.envs:RLTaskEnv",
kwargs={
"env_cfg_entry_point": ik_abs_env_cfg.FrankaCubeLiftEnvCfg_PLAY,
"rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
},
disable_env_checker=True,
)
##
# Inverse Kinematics - Relative Pose Control
##
gym.register(
id="Isaac-Lift-Cube-Franka-IK-Rel-v0",
entry_point="omni.isaac.orbit.envs:RLTaskEnv",
kwargs={
"env_cfg_entry_point": ik_rel_env_cfg.FrankaCubeLiftEnvCfg,
"rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
},
disable_env_checker=True,
)
gym.register(
id="Isaac-Lift-Cube-Franka-IK-Rel-Play-v0",
entry_point="omni.isaac.orbit.envs:RLTaskEnv",
kwargs={
"env_cfg_entry_point": ik_rel_env_cfg.FrankaCubeLiftEnvCfg_PLAY,
"rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
},
disable_env_checker=True,
......
source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/ik_abs_env_cfg.py 0 → 100644
View file @ cf7a65f3
# Copyright (c) 2022-2023, The ORBIT Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from omni.isaac.orbit.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from omni.isaac.orbit.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from omni.isaac.orbit.utils import configclass
from . import joint_pos_env_cfg
##
# Pre-defined configs
##
# isort: off
from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_HIGH_PD_CFG
@configclass
class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg):
def __post_init__(self):
# post init of parent
super().__post_init__()
# Set Franka as robot
# We switch here to a stiffer PD controller for IK tracking to be better.
self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# Set actions for the specific robot type (franka)
self.actions.body_joint_pos = DifferentialInverseKinematicsActionCfg(
asset_name="robot",
joint_names=["panda_joint.*"],
body_name="panda_hand",
controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"),
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
)
@configclass
class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg):
def __post_init__(self):
# post init of parent
super().__post_init__()
# make a smaller scene for play
self.scene.num_envs = 50
self.scene.env_spacing = 2.5
# disable randomization for play
self.observations.policy.enable_corruption = False
source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/ik_rel_env_cfg.py 0 → 100644
View file @ cf7a65f3
# Copyright (c) 2022-2023, The ORBIT Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from omni.isaac.orbit.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from omni.isaac.orbit.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from omni.isaac.orbit.utils import configclass
from . import joint_pos_env_cfg
##
# Pre-defined configs
##
# isort: off
from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_HIGH_PD_CFG
@configclass
class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg):
def __post_init__(self):
# post init of parent
super().__post_init__()
# Set Franka as robot
# We switch here to a stiffer PD controller for IK tracking to be better.
self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# Set actions for the specific robot type (franka)
self.actions.body_joint_pos = DifferentialInverseKinematicsActionCfg(
asset_name="robot",
joint_names=["panda_joint.*"],
body_name="panda_hand",
controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"),
scale=0.5,
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
)
@configclass
class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg):
def __post_init__(self):
# post init of parent
super().__post_init__()
# make a smaller scene for play
self.scene.num_envs = 50
self.scene.env_spacing = 2.5
# disable randomization for play
self.observations.policy.enable_corruption = False
source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/env_cfg.py → source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/joint_pos_env_cfg.py
View file @ cf7a65f3
......@@ -18,7 +18,7 @@ from omni.isaac.orbit_tasks.manipulation.lift.lift_env_cfg import LiftEnvCfg
# Pre-defined configs
##
# isort: off
from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_CFG
from omni.isaac.orbit.markers.config import FRAME_MARKER_CFG
......@@ -29,7 +29,7 @@ class FrankaCubeLiftEnvCfg(LiftEnvCfg):
super().__post_init__()
# Set Franka as robot
self.scene.robot = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# Set actions for the specific robot type (franka)
self.actions.body_joint_pos = mdp.JointPositionActionCfg(
......@@ -38,7 +38,7 @@ class FrankaCubeLiftEnvCfg(LiftEnvCfg):
self.actions.finger_joint_pos = mdp.BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=["panda_finger.*"],
open_command_expr={"panda_finger_.*": 0.3},
open_command_expr={"panda_finger_.*": 0.04},
close_command_expr={"panda_finger_.*": 0.0},
)
# Set the body name for the end effector
......
source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/reach/config/franka/env_cfg.py
View file @ cf7a65f3
......@@ -16,7 +16,7 @@ from omni.isaac.orbit_tasks.manipulation.reach.reach_env_cfg import ReachEnvCfg
# Pre-defined configs
##
# isort: off
from omni.isaac.orbit.assets.config import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
from omni.isaac.orbit.assets.config import FRANKA_PANDA_CFG
##
......@@ -31,7 +31,7 @@ class FrankaReachEnvCfg(ReachEnvCfg):
super().__post_init__()
# switch robot to franka
self.scene.robot = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# override rewards
self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["panda_hand"]
self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["panda_hand"]
......
source/standalone/demos/arms.py
View file @ cf7a65f3
......@@ -41,7 +41,7 @@ import omni.isaac.core.utils.prims as prim_utils
import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.assets import Articulation
from omni.isaac.orbit.assets.config import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG, UR10_CFG
from omni.isaac.orbit.assets.config import FRANKA_PANDA_CFG, UR10_CFG
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
......@@ -64,7 +64,7 @@ def design_scene() -> tuple[dict, list[list[float]]]:
cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd")
cfg.func("/World/Origin1/Table", cfg, translation=(0.55, 0.0, 1.05))
# -- Robot
franka_arm_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/Origin1/Robot")
franka_arm_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Origin1/Robot")
franka_arm_cfg.init_state.pos = (0.0, 0.0, 1.05)
robot_franka = Articulation(cfg=franka_arm_cfg)
......
source/standalone/environments/teleoperation/teleop_se3_agent.py
View file @ cf7a65f3
......@@ -66,9 +66,8 @@ def main():
# parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
# modify configuration
env_cfg.control.control_type = "inverse_kinematics"
env_cfg.control.inverse_kinematics.command_type = "pose_rel"
env_cfg.terminations.episode_timeout = False
env_cfg.terminations.time_out = None
# create environment
env = gym.make(args_cli.task, cfg=env_cfg)
# check environment name (for reach , we don't allow the gripper)
......@@ -107,12 +106,13 @@ def main():
with torch.inference_mode():
# get keyboard command
delta_pose, gripper_command = teleop_interface.advance()
delta_pose = delta_pose.astype("float32")
# convert to torch
delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=env.device).repeat(env.num_envs, 1)
delta_pose = torch.tensor(delta_pose, device=env.unwrapped.device).repeat(env.unwrapped.num_envs, 1)
# pre-process actions
actions = pre_process_actions(delta_pose, gripper_command)
# apply actions
_, _, _, _ = env.step(actions)
env.step(actions)
# close the simulator
env.close()
......
source/standalone/workflows/robomimic/collect_demonstrations.py
View file @ cf7a65f3
......@@ -135,7 +135,8 @@ def main():
# -- actions
collector_interface.add("actions", actions)
# perform action on environment
obs_dict, rewards, dones, info = env.step(actions)
obs_dict, rewards, terminated, truncated, info = env.step(actions)
dones = terminated | truncated
# check that simulation is stopped or not
if env.unwrapped.sim.is_stopped():
break
......
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