[Question] Operational Space Controller not working with UR10 arm #2775
Description
I am trying to build a task using a UR10 arm, that needs a compliant controller. However, using the OSC controller leads to very wild and rapid movements, and the agent also fails to learn anything. Please see the videos below for reference.
I tried swapping out the robot with a Franka in my environment, and I also tried swapping out the robot in the Franka Reach task (task id: Isaac-Reach-Franka-OSC-v0) with a UR10. In both these cases, the controller worked well with the Franka, and did not work with the UR10. I tried tuning the stiffness and damping gains, but to no avail.
To further isolate the issue, I replaced Franka with UR10 in run_osc.py script in the tutorials. Then I further simplified it by modifying run_osc.py to remove the titled table and contact sensors, and only have position targets, and the same issue persists.
I am unable to get the OSC to work with the UR10 arm. I'm not sure if it's a bug, or something I've overlooked in my implementation. Any help will be much appreciated.
Videos: -
-
Modified run_osc script with UR10
https://github.com/user-attachments/assets/08960dfe-0875-4dfd-8e1d-8567b469368a -
Same modified run_osc script with Franka
https://github.com/user-attachments/assets/6692e251-d364-432b-a50e-147251abf47a -
UR10 in the environment I created for my task. I created this environment by using the Reach as a template and placing a hole that acts as the reach goal. Swapping out UR10 with Franka works fine in this too.
https://github.com/user-attachments/assets/9e1fe493-f0e2-485f-a2c8-69d6199f0906
Below is the modified run_osc script to replicate the issue. To replicate the issue on the reach task, just replace FRANKA_PANDA_CFG with UR10_CFG and change the link names in osc_env_cfg.py in the Franka Reach task.
import argparse
from isaaclab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="Tutorial on using the operational space controller.")
parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to spawn.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
"""Rest everything follows."""
import torch
import isaaclab.sim as sim_utils
from isaaclab.assets import Articulation, AssetBaseCfg
from isaaclab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils import configclass
from isaaclab.utils.math import (
combine_frame_transforms,
matrix_from_quat,
quat_apply_inverse,
quat_inv,
subtract_frame_transforms,
)
##
# Pre-defined configs
##
from isaaclab_assets import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG # isort:skip
@configclass
class SceneCfg(InteractiveSceneCfg):
"""Configuration for a simple scene with a tilted wall."""
# ground plane
ground = AssetBaseCfg(
prim_path="/World/defaultGroundPlane",
spawn=sim_utils.GroundPlaneCfg(),
)
# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)
# Tilted wall
# tilted_wall = AssetBaseCfg(
# prim_path="{ENV_REGEX_NS}/TiltedWall",
# spawn=sim_utils.CuboidCfg(
# size=(2.0, 1.5, 0.01),
# collision_props=sim_utils.CollisionPropertiesCfg(),
# visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1),
# rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True),
# activate_contact_sensors=True,
# ),
# init_state=AssetBaseCfg.InitialStateCfg(
# pos=(0.6 + 0.085, 0.0, 0.3), rot=(0.9238795325, 0.0, -0.3826834324, 0.0)
# ),
# )
# contact_forces = ContactSensorCfg(
# prim_path="/World/envs/env_.*/TiltedWall",
# update_period=0.0,
# history_length=2,
# debug_vis=False,
# )
robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
robot.actuators["panda_shoulder"].stiffness = 0.0
robot.actuators["panda_shoulder"].damping = 0.0
robot.actuators["panda_forearm"].stiffness = 0.0
robot.actuators["panda_forearm"].damping = 0.0
robot.spawn.rigid_props.disable_gravity = True
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
"""Runs the simulation loop.
Args:
sim: (SimulationContext) Simulation context.
scene: (InteractiveScene) Interactive scene.
"""
# Extract scene entities for readability.
robot = scene["robot"]
# contact_forces = scene["contact_forces"]
# Obtain indices for the end-effector and arm joints
ee_frame_name = "panda_leftfinger"
arm_joint_names = ["panda_joint.*"]
ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0]
arm_joint_ids = robot.find_joints(arm_joint_names)[0]
# Create the OSC
osc_cfg = OperationalSpaceControllerCfg(
target_types=["pose_abs"],
# impedance_mode="variable_kp",
inertial_dynamics_decoupling=True,
partial_inertial_dynamics_decoupling=False,
gravity_compensation=False,
motion_damping_ratio_task=1.0,
motion_stiffness_task=100,
# contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0],
# motion_control_axes_task=[1, 1, 0, 1, 1, 1],
# contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0],
# nullspace_control="position",
)
osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device)
# Markers
frame_marker_cfg = FRAME_MARKER_CFG.copy()
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
# Define targets for the arm
ee_goal_pose_set_tilted_b = torch.tensor(
[
[0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343],
[0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343],
[0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343],
],
device=sim.device,
)
# ee_goal_wrench_set_tilted_task = torch.tensor(
# [
# [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
# [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
# [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
# ],
# device=sim.device,
# )
# kp_set_task = torch.tensor(
# [
# [360.0, 360.0, 360.0, 360.0, 360.0, 360.0],
# [420.0, 420.0, 420.0, 420.0, 420.0, 420.0],
# [320.0, 320.0, 320.0, 320.0, 320.0, 320.0],
# ],
# device=sim.device,
# )
ee_target_set = torch.cat([ee_goal_pose_set_tilted_b], dim=-1)
# Define simulation stepping
sim_dt = sim.get_physics_dt()
# Update existing buffers
# Note: We need to update buffers before the first step for the controller.
robot.update(dt=sim_dt)
# Get the center of the robot soft joint limits
joint_centers = torch.mean(robot.data.soft_joint_pos_limits[:, arm_joint_ids, :], dim=-1)
# get the updated states
(
jacobian_b,
mass_matrix,
gravity,
ee_pose_b,
ee_vel_b,
root_pose_w,
ee_pose_w,
# ee_force_b,
joint_pos,
joint_vel,
) = update_states(sim, scene, robot, ee_frame_idx, arm_joint_ids)
# Track the given target command
current_goal_idx = 0 # Current goal index for the arm
command = torch.zeros(
scene.num_envs, osc.action_dim, device=sim.device
) # Generic target command, which can be pose, position, force, etc.
ee_target_pose_b = torch.zeros(scene.num_envs, 7, device=sim.device) # Target pose in the body frame
ee_target_pose_w = torch.zeros(scene.num_envs, 7, device=sim.device) # Target pose in the world frame (for marker)
# Set joint efforts to zero
zero_joint_efforts = torch.zeros(scene.num_envs, robot.num_joints, device=sim.device)
joint_efforts = torch.zeros(scene.num_envs, len(arm_joint_ids), device=sim.device)
count = 0
# Simulation loop
while simulation_app.is_running():
# reset every 500 steps
if count % 500 == 0:
# reset joint state to default
default_joint_pos = robot.data.default_joint_pos.clone()
default_joint_vel = robot.data.default_joint_vel.clone()
robot.write_joint_state_to_sim(default_joint_pos, default_joint_vel)
robot.set_joint_effort_target(zero_joint_efforts) # Set zero torques in the initial step
robot.write_data_to_sim()
robot.reset()
# reset contact sensor
# contact_forces.reset()
# reset target pose
robot.update(sim_dt)
_, _, _, ee_pose_b, _, _, _, _, _ = update_states(
sim, scene, robot, ee_frame_idx, arm_joint_ids
) # at reset, the jacobians are not updated to the latest state
command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = update_target(
sim, scene, osc, root_pose_w, ee_target_set, current_goal_idx
)
# set the osc command
osc.reset()
command, task_frame_pose_b = convert_to_task_frame(osc, command=command, ee_target_pose_b=ee_target_pose_b)
osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b)
else:
# get the updated states
(
jacobian_b,
mass_matrix,
gravity,
ee_pose_b,
ee_vel_b,
root_pose_w,
ee_pose_w,
# ee_force_b,
joint_pos,
joint_vel,
) = update_states(sim, scene, robot, ee_frame_idx, arm_joint_ids)
# compute the joint commands
joint_efforts = osc.compute(
jacobian_b=jacobian_b,
current_ee_pose_b=ee_pose_b,
current_ee_vel_b=ee_vel_b,
# current_ee_force_b=ee_force_b,
mass_matrix=mass_matrix,
gravity=gravity,
current_joint_pos=joint_pos,
current_joint_vel=joint_vel,
nullspace_joint_pos_target=joint_centers,
)
# apply actions
robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids)
robot.write_data_to_sim()
# update marker positions
ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
goal_marker.visualize(ee_target_pose_w[:, 0:3], ee_target_pose_w[:, 3:7])
# perform step
sim.step(render=True)
# update robot buffers
robot.update(sim_dt)
# update buffers
scene.update(sim_dt)
# update sim-time
count += 1
# Update robot states
def update_states(
sim: sim_utils.SimulationContext,
scene: InteractiveScene,
robot: Articulation,
ee_frame_idx: int,
arm_joint_ids: list[int],
# contact_forces,
):
"""Update the robot states.
Args:
sim: (SimulationContext) Simulation context.
scene: (InteractiveScene) Interactive scene.
robot: (Articulation) Robot articulation.
ee_frame_idx: (int) End-effector frame index.
arm_joint_ids: (list[int]) Arm joint indices.
contact_forces: (ContactSensor) Contact sensor.
Returns:
jacobian_b (torch.tensor): Jacobian in the body frame.
mass_matrix (torch.tensor): Mass matrix.
gravity (torch.tensor): Gravity vector.
ee_pose_b (torch.tensor): End-effector pose in the body frame.
ee_vel_b (torch.tensor): End-effector velocity in the body frame.
root_pose_w (torch.tensor): Root pose in the world frame.
ee_pose_w (torch.tensor): End-effector pose in the world frame.
ee_force_b (torch.tensor): End-effector force in the body frame.
joint_pos (torch.tensor): The joint positions.
joint_vel (torch.tensor): The joint velocities.
Raises:
ValueError: Undefined target_type.
"""
# obtain dynamics related quantities from simulation
ee_jacobi_idx = ee_frame_idx - 1
jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids]
mass_matrix = robot.root_physx_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids]
gravity = robot.root_physx_view.get_gravity_compensation_forces()[:, arm_joint_ids]
# Convert the Jacobian from world to root frame
jacobian_b = jacobian_w.clone()
root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w))
jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :])
jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :])
# Compute current pose of the end-effector
root_pos_w = robot.data.root_pos_w
root_quat_w = robot.data.root_quat_w
ee_pos_w = robot.data.body_pos_w[:, ee_frame_idx]
ee_quat_w = robot.data.body_quat_w[:, ee_frame_idx]
ee_pos_b, ee_quat_b = subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w)
root_pose_w = torch.cat([root_pos_w, root_quat_w], dim=-1)
ee_pose_w = torch.cat([ee_pos_w, ee_quat_w], dim=-1)
ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1)
# Compute the current velocity of the end-effector
ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame
root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame
relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame
ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame
ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6])
ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1)
# # Calculate the contact force
# ee_force_w = torch.zeros(scene.num_envs, 3, device=sim.device)
# sim_dt = sim.get_physics_dt()
# contact_forces.update(sim_dt) # update contact sensor
# # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and
# # taking the max of three surfaces as only one should be the contact of interest
# ee_force_w, _ = torch.max(torch.mean(contact_forces.data.net_forces_w_history, dim=1), dim=1)
# This is a simplification, only for the sake of testing.
# ee_force_b = ee_force_w
# Get joint positions and velocities
joint_pos = robot.data.joint_pos[:, arm_joint_ids]
joint_vel = robot.data.joint_vel[:, arm_joint_ids]
return (
jacobian_b,
mass_matrix,
gravity,
ee_pose_b,
ee_vel_b,
root_pose_w,
ee_pose_w,
# ee_force_b,
joint_pos,
joint_vel,
)
# Update the target commands
def update_target(
sim: sim_utils.SimulationContext,
scene: InteractiveScene,
osc: OperationalSpaceController,
root_pose_w: torch.tensor,
ee_target_set: torch.tensor,
current_goal_idx: int,
):
"""Update the targets for the operational space controller.
Args:
sim: (SimulationContext) Simulation context.
scene: (InteractiveScene) Interactive scene.
osc: (OperationalSpaceController) Operational space controller.
root_pose_w: (torch.tensor) Root pose in the world frame.
ee_target_set: (torch.tensor) End-effector target set.
current_goal_idx: (int) Current goal index.
Returns:
command (torch.tensor): Updated target command.
ee_target_pose_b (torch.tensor): Updated target pose in the body frame.
ee_target_pose_w (torch.tensor): Updated target pose in the world frame.
next_goal_idx (int): Next goal index.
Raises:
ValueError: Undefined target_type.
"""
# update the ee desired command
command = torch.zeros(scene.num_envs, osc.action_dim, device=sim.device)
command[:] = ee_target_set[current_goal_idx]
# update the ee desired pose
ee_target_pose_b = torch.zeros(scene.num_envs, 7, device=sim.device)
for target_type in osc.cfg.target_types:
if target_type == "pose_abs":
ee_target_pose_b[:] = command[:, :7]
elif target_type == "wrench_abs":
pass # ee_target_pose_b could stay at the root frame for force control, what matters is ee_target_b
else:
raise ValueError("Undefined target_type within update_target().")
# update the target desired pose in world frame (for marker)
ee_target_pos_w, ee_target_quat_w = combine_frame_transforms(
root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7]
)
ee_target_pose_w = torch.cat([ee_target_pos_w, ee_target_quat_w], dim=-1)
next_goal_idx = (current_goal_idx + 1) % len(ee_target_set)
return command, ee_target_pose_b, ee_target_pose_w, next_goal_idx
# Convert the target commands to the task frame
def convert_to_task_frame(osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor):
"""Converts the target commands to the task frame.
Args:
osc: OperationalSpaceController object.
command: Command to be converted.
ee_target_pose_b: Target pose in the body frame.
Returns:
command (torch.tensor): Target command in the task frame.
task_frame_pose_b (torch.tensor): Target pose in the task frame.
Raises:
ValueError: Undefined target_type.
"""
command = command.clone()
task_frame_pose_b = ee_target_pose_b.clone()
cmd_idx = 0
for target_type in osc.cfg.target_types:
if target_type == "pose_abs":
command[:, :3], command[:, 3:7] = subtract_frame_transforms(
task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7]
)
cmd_idx += 7
elif target_type == "wrench_abs":
# These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is
# easier), so not transforming
cmd_idx += 6
else:
raise ValueError("Undefined target_type within _convert_to_task_frame().")
return command, task_frame_pose_b
def main():
"""Main function."""
# Load kit helper
sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
sim = sim_utils.SimulationContext(sim_cfg)
# Set main camera
sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
# Design scene
scene_cfg = SceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
scene = InteractiveScene(scene_cfg)
# Play the simulator
sim.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# Run the simulator
run_simulator(sim, scene)
if __name__ == "__main__":
# run the main function
main()
# close sim app
simulation_app.close()