Isaac Sim Version
4.0.0
Operating System
Ubuntu 22.04
GPU Information
- Model: RTX3080
- Driver Version: 550.120
Topic Description
I have been setting up curobo as a solver for our custom robot at work. I am using a modified multi_arm_reacher.py although the core of code is same. I have pasted the file if needed.
Everything works fine, except for one error that stops the entire simulaiton and I need to restart the simulation: MotionGenStatus.INVALID_START_STATE_JOINT_LIMITS. This doesn’t happen at the beginning, but rather when in the previous generated motion, one of the joints exceeds the joint limits.
I’ve been printing joint positions to confirm. I would’ve expected that the solver would not generate a path which would take some joints beyond their limits to begin with. Can you help me understand why this is happening? Is there a way to avoid this or increase the joint limit tolerances?
Thank You.
code:
try:
# Third Party
import isaacsim
except ImportError:
pass
# Third Party
import torch
import zmq
import time
a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--robot", type=str, default="dual_ur10e.yml", help="robot configuration to load"
)
args = parser.parse_args()
# Third Party
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
############################################################
# Third Party
import carb
import numpy as np
from omni.isaac.core import World
from omni.isaac.core.objects import cuboid, sphere
########### OV #################
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.articulations import Articulation
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
import os
import sys
sys.path.append(os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(os.path.__file__)))))))
print(f'PATH: {sys.path}')
from helper import add_extensions, add_robot_to_scene
class MultiArmControl:
def __init__(self, transmit_data = False):
self.transmit_data = transmit_data # to send current js
self.setup()
def setup(self):
# assuming obstacles are in objects_path:
self.my_world = World(stage_units_in_meters=1.0)
self.stage = self.my_world.stage
self.xform = self.stage.DefinePrim("/World", "Xform")
self.stage.SetDefaultPrim(self.xform)
self.stage.DefinePrim("/curobo", "Xform")
# my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
self.stage = self.my_world.stage
# stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
# Make a target to follow
setup_curobo_logger("warn")
self.past_pose = None
self.n_obstacle_cuboids = 30
self.n_obstacle_mesh = 10
# warmup curobo instance
self.usd_help = UsdHelper()
self.target_pose = None
self.tensor_args = TensorDeviceType()
self.robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
self.j_names = self.robot_cfg["kinematics"]["cspace"]["joint_names"]
self.default_config = self.robot_cfg["kinematics"]["cspace"]["retract_config"]
# self.robot_name = "robot_{}".format(np.random.randint(0, 1000)) # Generate a unique robot name
self.robot, self.robot_prim_path = add_robot_to_scene(self.robot_cfg, self.my_world)#, robot_name=self.robot_name)
self.articulation_controller = self.robot.get_articulation_controller()
self.world_cfg_table = WorldConfig.from_dict(
load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
)
self.world_cfg_table.cuboid[0].pose[2] -= 0.02
self.world_cfg1 = WorldConfig.from_dict(
load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
).get_mesh_world()
self.world_cfg1.mesh[0].name += "_mesh"
self.world_cfg1.mesh[0].pose[2] = -10.5
self.world_cfg = WorldConfig(cuboid=self.world_cfg_table.cuboid, mesh=self.world_cfg1.mesh)
self.motion_gen_config = MotionGenConfig.load_from_robot_config(
self.robot_cfg,
self.world_cfg,
self.tensor_args,
collision_checker_type=CollisionCheckerType.MESH,
use_cuda_graph=True,
interpolation_dt=0.03,
collision_cache={"obb": self.n_obstacle_cuboids, "mesh": self.n_obstacle_mesh},
collision_activation_distance=0.025,
fixed_iters_trajopt=True,
maximum_trajectory_dt=0.5,
position_threshold = 0.5,
rotation_threshold = 0.5
)
self.motion_gen = MotionGen(self.motion_gen_config)
print("warming up...")
self.motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
print("Curobo is Ready")
add_extensions(simulation_app, args.headless_mode)
self.plan_config = MotionGenPlanConfig(
enable_graph=False,
enable_graph_attempt=4,
max_attempts=10,
time_dilation_factor=0.8,
)
self.usd_help.load_stage(self.my_world.stage)
self.usd_help.add_world_to_stage(self.world_cfg, base_frame="/World")
self.cmd_plan = None
self.cmd_idx = 0
self.my_world.scene.add_default_ground_plane()
self.i = 0
self.spheres = None
# read number of targets in link names:
self.link_names = self.motion_gen.kinematics.link_names
self.ee_link_name = self.motion_gen.kinematics.ee_link
# get link poses at retract configuration:
self.kin_state = self.motion_gen.kinematics.get_state(self.motion_gen.get_retract_config().view(1, -1))
self.link_retract_pose = self.kin_state.link_pose
self.t_pos = np.ravel(self.kin_state.ee_pose.to_list())
self.target = cuboid.VisualCuboid(
"/World/target",
position=self.t_pos[:3],
orientation=self.t_pos[3:],
color=np.array([1.0, 0, 0]),
size=0.05,
)
# create new targets for new links:
self.ee_idx = self.link_names.index(self.ee_link_name)
self.target_links = {}
self.names = []
for i in self.link_names:
if i != self.ee_link_name:
k_pose = np.ravel(self.link_retract_pose[i].to_list())
color = np.random.randn(3) * 0.2
color[0] += 0.5
color[1] = 0.5
color[2] = 0.0
self.target_links[i] = cuboid.VisualCuboid(
"/World/target_" + i,
position=np.array(k_pose[:3]),
orientation=np.array(k_pose[3:]),
color=color,
size=0.05,
)
self.names.append("/World/target_" + i)
self.i = 0
self.step_index = 0
self.left_ee_prim = Articulation(prim_path = "/RoboticArms/left_ee_link")
self.right_ee_prim = Articulation(prim_path = "/RoboticArms/right_ee_link")
if self.transmit_data:
self.zmq_publisher = zmq.Context()
self.zmq_publisher = self.zmq_publisher.socket(zmq.REP)
self.zmq_publisher.bind("tcp://*:5575")
def step_ik(self,
target_position_left=None,
target_orientation_left=None,
target_position_right=None,
target_orientation_right=None):
# left arm control:
if target_position_left is not None:
print("setting left target")
self.target_links["left_ee_link"].set_world_pose(position=target_position_left, orientation=target_orientation_left)
# right arm control:
if target_position_right is not None:
print("setting right target")
self.target.set_world_pose(position=target_position_right, orientation=target_orientation_right)
# position and orientation of target virtual cube:
cube_position, cube_orientation = self.target.get_world_pose()
# print(f'cube position: {cube_position} cube orientation: {cube_orientation}')
# for i in self.target_links.keys():
# c_p, c_rot = self.target_links[i].get_world_pose()
# print(f'c_p: {c_p} c_rot: {c_rot}')
right_ee_pos, right_ee_or = self.right_ee_prim.get_world_pose()
left_ee_pos, left_ee_or = self.left_ee_prim.get_world_pose()
# print(f'right pos: {right_ee_pos} right or: {right_ee_or}')
# print(f'left pos: {left_ee_pos} left or: {left_ee_or}')
if self.past_pose is None:
self.past_pose = cube_position
if self.target_pose is None:
self.target_pose = cube_position
sim_js = self.robot.get_joints_state()
sim_js_names = self.robot.dof_names
cu_js = JointState(
position=self.tensor_args.to_device(sim_js.positions),
velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0,
acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0,
jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0,
joint_names=sim_js_names,
)
cu_js = cu_js.get_ordered_joint_state(self.motion_gen.kinematics.joint_names)
print(sim_js_names)
print(sim_js.positions)
# print(sim_js.velocities)
# print(f"MAX: {np.max(np.abs(sim_js.velocities))}")
if (
np.linalg.norm(cube_position - self.target_pose) > 1e-6
# and np.linalg.norm(past_pose - cube_position) == 0.0
# and np.max(np.abs(sim_js.velocities)) < 0.2
):
print("1")
# Set EE teleop goals, use cube for simple non-vr init:
ee_translation_goal = cube_position
ee_orientation_teleop_goal = cube_orientation
print("2")
# compute curobo solution:
ik_goal = Pose(
position=self.tensor_args.to_device(ee_translation_goal),
quaternion=self.tensor_args.to_device(ee_orientation_teleop_goal),
)
print("3")
# add link poses:
link_poses = {}
for i in self.target_links.keys():
c_p, c_rot = self.target_links[i].get_world_pose()
# print(f'c_p: {c_p} c_rot: {c_rot}')
link_poses[i] = Pose(
position=self.tensor_args.to_device(c_p),
quaternion=self.tensor_args.to_device(c_rot),
)
print("4")
result = self.motion_gen.plan_single(
cu_js.unsqueeze(0), ik_goal, self.plan_config.clone(), link_poses=link_poses
)
# ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1))
print("5")
succ = result.success.item() # ik_result.success.item()
print("6")
if succ:
self.cmd_plan = result.get_interpolated_plan()
self.cmd_plan = self.motion_gen.get_full_js(self.cmd_plan)
# get only joint names that are in both:
self.idx_list = []
common_js_names = []
for x in sim_js_names:
if x in self.cmd_plan.joint_names:
self.idx_list.append(self.robot.get_dof_index(x))
common_js_names.append(x)
# idx_list = [robot.get_dof_index(x) for x in sim_js_names]
self.cmd_plan = self.cmd_plan.get_ordered_joint_state(common_js_names)
self.cmd_idx = 0
print("7")
else:
carb.log_warn("Plan did not converge to a solution: " + str(result.status))
print("8")
self.target_pose = cube_position
print("9")
self.past_pose = cube_position
# print("10")
if self.cmd_plan is not None:
self.cmd_state = self.cmd_plan[self.cmd_idx]
# get full dof state
art_action = ArticulationAction(
self.cmd_state.position.cpu().numpy(),
self.cmd_state.velocity.cpu().numpy(),
joint_indices=self.idx_list,
)
print("11")
self.art_action = art_action
# print(f'art_action: {art_action}')
# set desired joint angles obtained from IK:
self.articulation_controller.apply_action(art_action)
########### UPDATE for Req-Rep
# if self.transmit_data:
# self.zmq_publisher.send_pyobj(art_action)
print("12")
self.cmd_idx += 1
for _ in range(2):
self.my_world.step(render=False)
if self.cmd_idx >= len(self.cmd_plan.position):
self.cmd_idx = 0
self.cmd_plan = None
print("13")
def test_general():
MultiArmController = MultiArmControl()
while simulation_app.is_running():
try:
# ------------------ Run Once ------------------
MultiArmController.my_world.step(render=True)
if not MultiArmController.my_world.is_playing():
if MultiArmController.i % 100 == 0:
print("**** Click Play to start simulation *****")
MultiArmController.i += 1
# if step_index == 0:
# my_world.play()
continue
MultiArmController.step_index = MultiArmController.my_world.current_time_step_index
if MultiArmController.step_index <= 2:
MultiArmController.my_world.reset()
MultiArmController.idx_list = [MultiArmController.robot.get_dof_index(x) for x in MultiArmController.j_names]
MultiArmController.robot.set_joint_positions(MultiArmController.default_config, MultiArmController.idx_list)
MultiArmController.robot._articulation_view.set_max_efforts(
values=np.array([5000 for MultiArmController.i in range(len(MultiArmController.idx_list))]), joint_indices=MultiArmController.idx_list
)
if MultiArmController.step_index < 20:
continue
if MultiArmController.step_index == 50 or MultiArmController.step_index % 1000 == 0.0:
print("Updating world, reading w.r.t.", MultiArmController.robot_prim_path)
obstacles = MultiArmController.usd_help.get_obstacles_from_stage(
# only_paths=[obstacles_path],
reference_prim_path=MultiArmController.robot_prim_path,
ignore_substring=[
MultiArmController.robot_prim_path,
"/World/target",
"/World/defaultGroundPlane",
"/curobo",
]
+ MultiArmController.names,
).get_collision_check_world()
MultiArmController.motion_gen.update_world(obstacles)
print("Updated World")
carb.log_info("Synced CuRobo world from stage.")
# -------------------- ARM CONTROL --------------------
#set world pose
# print("going into step_ik")
MultiArmController.step_ik()
# print("getting out of step_ik")