running plan_single() with franka arm at positions other than origin fails

[Abhijit16]

I am trying to use MotionGen.plan_single() in cuRobo's motion planning using Franka panda robot arm. It works fine when arm is kept at origin(0,0,0); however am running into error(MotionGenStatus.IK_Fail), when I shift the position of the arm at an offset from the world origin. I use som of Pose's inbuilt functions to compute the target cube's(a green cube named 'goalCube') new Pose with respect to the base of the arm, whose code is as follows:

`
"""
robot_base_pose_world: the robot's new base pose in the world frame.
target_pose_world: end-effector pose in the real world.
"""
def convert_target_pose_wrt_robot_base(robot_base_pose_world:Pose, target_pose_world:Pose):
# Calculate the inverse(pose) of the robot's base frame.
robot_base_inv:Pose = robot_base_pose_world.inverse()

# Transform the world target pose into the robot's local frame.
target_pose_local = robot_base_inv.multiply(target_pose_world)

return target_pose_local

`

This function takes in the original target pose(goal_pose) & return a new pose (goal_pose1). Now, when i supply this goal_pose1 to plan_single(), it results in IK_Fail error. During debugging, I observed that the Z position of goal_pose1 goes negative(from 0.2298 to -0.1443). I tried changing this value to several other values & finally saw that setting this to a small negative value (say,-0.01) makes plan_single() work, & the arm goes near the target cube successfully. The below code contains all these:

compute() is the main function, world_cfg() generates the world-config to be supplied to MotionGen().

`

this function returns the world-config dictionary.

def world_cfg():
stage = omni.usd.get_context().get_stage() # get the current USD stage.
usd_helper = UsdHelper() # Create an instance of UsdHelper.

world_config = usd_helper.load_stage(stage)   # Load the stage into UsdHelper and generate WorldConfig.
#print(f"abhijit - stage & usd_helper: {stage}, {usd_helper}")

#"""
# WorldConfig object is a dictionary-based representation of the obstacles.
ignore_substrings = [
"/World/Ground",
#"/World/goalCube",
"/World/franka",  # Replace with the actual prim path for your robot
]
# Extract obstacles from the USD stage. The 'reference_prim_path' should be the path to your robot's base link. cuRobo transforms the obstacles relative to the robot's base frame.
world_config_dict = usd_helper.get_obstacles_from_stage(
    only_paths=["/World"],
    reference_prim_path="/World/franka",
    #reference_prim_path="/World/franka/panda_link0",
    ignore_substring=ignore_substrings
).get_collision_check_world()
#print(f"world_config_dict: {world_config_dict}")  # lots of data.
#with open("C:\\Users\\abhij\\Desktop\\IsaacSim_programs_files\\franka\\world_config_dict.json", "w") as f:
#    f.write(str(world_config_dict))
#"""
return world_config_dict

def compute(db: og.Database):
#db.outputs.trajectory = []
# (1) generate WorldConfig from current stage.
world_config = world_cfg()

#franka_prim = db.inputs.franka_prim  # data type:IBundle2.

# (2) load config for franka, from .yml config file, to generate a 'motion generation config' data.
motion_gen_config = MotionGenConfig.load_from_robot_config("franka.yml", world_config, interpolation_dt=0.01,)
#print(f"Abhijit: motion_gen_config: {motion_gen_config}")  # generates a huge data structure.
#with open("C:\\Users\\abhij\\Desktop\\IsaacSim_programs_files\\franka\\motion_gen_config.json", "w") as f:
#    f.write(str(motion_gen_config))   # for debug.

# prepare for motion planning.
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()
motion_gen.update_world(world_config)

# Articulation state is used to get the jonits values for a robot's joints, when it's prim is set to that robot.
# https://docs.isaacsim.omniverse.nvidia.com/latest/py/source/extensions/isaacsim.core.nodes/docs/ogn/OgnIsaacArticulationState.html
# Note that Articulation State node's exec:In pin is connected to Playback-Tick node, so that updated joint state's are obtained at any given frame.
# Articulation State node's 'joint positions' returns these 9 values(in radians) [0.012, -0.569988, 1.92081e-09, -2.81, 1.51394e-07, 3.037, 0.741, 0.0399181, 0.0399181] during debug printing. These values correspond to the 'Target Position' values of the joints(visible in degree values) in 'Drive'->'Angular' of the joint's property panel.
franka_joints_state = db.inputs.franka_joints_state     # type: numpy-ndarray
# convert franka_joints_state to torch tensor on cuda. Supply all joint values, except gripper joints(i.e. last 2 joints).
# NOTE: the joints values should have shape:[1,<number of joints>](hence use unsqueeze()); & should be of type 'float32'(hence use to(dtype=)).
franka_joints_state_tensor = torch.from_numpy(franka_joints_state[:-2]).to(dtype=torch.float32).unsqueeze(dim=0)

# define the start state. 'franka_joint_state' provides the current joint state of the arm.
tensor_args = TensorDeviceType(device=device)  # device type = 'device'.
start_state = JointState.from_position(
position=franka_joints_state_tensor,
# franka panda has 8 joints(panda_joint8, panda_hand_joint are fixed joints). gripper joints are not used here(panda_finger_joint2 is mimic joint).
joint_names=["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
    "panda_joint5", "panda_joint6", "panda_joint7",
    #"panda_finger_joint1",
    ],
).to(tensor_args)   # this needs to be on cuda, for plan_single() to work.

# debug -draw collision spheres.
#print(f"Abhijit: franka_joints_state_tensor: {franka_joints_state_tensor}")
#print(f"Abhijit: franka_joints_state_tensor shape: {franka_joints_state_tensor.shape}")  # [1,7]
#draw_collision_spheres(motion_gen, start_state)

#print(f"Abhijit: start_state: {start_state}")

# get goal position & orientation, as the final position to move the arm to.
goal_position = db.inputs.goal_position   #[x,y,z]
goal_orientation = db.inputs.goal_orientation    # [x,y,z,w]
#print(f"pos:{goal_position}, orient:{goal_orientation}")
# Goal pose for the end-effector: generate aggregate goal pose from goal object's translation & orientation. Note the order of orientation supplied i.e. w,x,y,z.
goal_pose = Pose.from_list([goal_position[0], goal_position[1], goal_position[2], goal_orientation[3], goal_orientation[0], goal_orientation[1], goal_orientation[2]])  # x, y, z, qw, qx, qy, qz.
#print(f"Abhijit: goal_pose: {goal_pose}")

# NOTE: Franka robotic arm needs to be some distance(~0.14) away from any obstacle, for motion planner to work without throwing 'Collision/IK Failure' errors.

# get franka prim's world Pose.
franka_prim = XFormPrim("/World/franka")
franka_position, franka_orientation = franka_prim.get_world_pose()
robot_base_pose_world = Pose(position=torch.tensor(franka_position), quaternion=torch.tensor(franka_orientation), name='panda_hand').to(device=device)  # needs to be on cuda, if cuda is available, like other Pose objects.
#print(f"Abhijit:robot_base_pose_world: {robot_base_pose_world}")

goal_pose1 = convert_target_pose_wrt_robot_base(robot_base_pose_world, goal_pose)

print(f"Abhijit:goal_pose: {goal_pose}")
print(f"Abhijit:goal_pose1: {goal_pose1}")

# method1: generate motion plan - plan_single() generates a single, optimized trajectory to reach a specified goal pose.
"""
goal_pose.position= [[0.1728, 0.1954, 0.2298]]
goal_pose1.position=[[ 0.1728,  0.5329, -0.1443]]
"""
#goal_pose1.position = goal_pose.position   # works for this. means position values are creating problem ?
goal_pose1.position[0,2] = -0.01 # this works too. larger -ve values(-0.1) seem to create problems ? smaller -ve values seem to work(-0.01).

result = motion_gen.plan_single(start_state, goal_pose1, MotionGenPlanConfig(max_attempts=1))

# in above code, goal_pose works, but goal_pose1 does not(IK failure). why ?

# method2: generate complete plan for moving & grasping a target. plan_grasp() plans a full sequence of motions that includes an approach, the grasp itself, and a retraction.
"""
result = execute_plan_grasp(motion_gen, goal_position, goal_orientation, start_state)
print("Abhijit - completed")
return True
"""

print(f"Abhijit: number of obstacles: {len(world_config.objects)}")
print("Abhijit result success: ", result.success.item())
#print("Abhijit result: ", result)

# trajectory info.
if result.success.item():
    # trajectory is basically the JointState(that contains the position, velocity, joint-names, acceleration, jerk).
    traj = result.get_interpolated_plan()  # result.interpolation_dt has the dt between timesteps.
    
    #print(f"traj_position: {traj.position.shape}")  # ex: [144,7] - this means the trajectory contains joint positions to reach the target pose, in the next 144 sub-paths.
    traj = traj.get_ordered_joint_state(traj.joint_names)
    db.outputs.joint_names = traj.joint_names   # return the joint names.
    db.outputs.trajectory = traj.position.cpu()   # return the output trajectory representing the motion plan.
    db.outputs.traj_shape = list(traj.position.shape)  # return the shape of this trajectory.
else:
    print(f"Abhijit: Status: {result.status}")
    print(f"Abhijit result failure details: {result}")  # print result details, if failed.
    db.outputs.trajectory = []   # set output to empty list if motion planning failed.
    db.outputs.traj_shape = []
return True

`

The new position of the target cube with respect to the base of the arm is correct, I verified it by creating a sample parent prim at base of the arm, & creating another child prim & setting it's value to the goal_pose1 (x,y,z) values, & it correctly superimposed on the target cube.

I am not sure why this happens, is there something else that needs to be done in the convert_target_pose_wrt_robot_base() function, or elsewhere ?
Kindly do help me.

My cuda version is 12.1.
cuRobo installation mode: isaacsim
python(using in conda env) version: 3.10.16
GPU: 4080 super
OS: Windows(venv is anaconda)
Isaacsim version: 4.5.0

Log:

2025-09-29 06:38:33 [Info] [omni.kit.app._impl] [py stdout]: Abhijit:goal_pose: Pose(position=tensor([[0.1728, 0.3770, 0.2298]], device='cuda:0'), quaternion=tensor([[1., 0., 0., 0.]], device='cuda:0'), rotation=None, batch=1, n_goalset=1, name='ee_link', normalize_rotation=True)
2025-09-29 06:38:33 [Info] [omni.kit.app._impl] [py stdout]: Abhijit:goal_pose1: Pose(position=tensor([[ 0.1728, 0.7144, -0.1443]], device='cuda:0'), quaternion=tensor([[1., 0., 0., 0.]], device='cuda:0'), rotation=None, batch=1, n_goalset=1, name='ee_link', normalize_rotation=True)
2025-09-29 06:38:33 [Info] [curobo] Planning for Single Goal: 1
2025-09-29 06:38:33 [Info] [curobo] Updating state_seq buffer reference (created new tensor)
2025-09-29 06:38:33 [Info] [curobo] MG Iter: 0
2025-09-29 06:38:33 [Info] [curobo] Updating safety params
2025-09-29 06:38:33 [Info] [curobo] Updating optimizer params
2025-09-29 06:38:33 [Info] [omni.kit.app._impl] [py stdout]: Abhijit: number of obstacles: 2
2025-09-29 06:38:33 [Info] [omni.kit.app._impl] [py stdout]: Abhijit result success: False
2025-09-29 06:38:33 [Info] [omni.kit.app._impl] [py stdout]: Abhijit: Status: MotionGenStatus.IK_FAIL
2025-09-29 06:38:33 [Info] [omni.kit.app._impl] [py stdout]: Abhijit result failure details: MotionGenResult(success=tensor([False], device='cuda:0'), valid_query=True, optimized_plan=None, optimized_dt=None, position_error=None, rotation_error=None, cspace_error=None, solve_time=0.0065000057220458984, ik_time=0.0065000057220458984, graph_time=0.0, trajopt_time=0.0, finetune_time=0.0, total_time=0.011499881744384766, interpolated_plan=None, interpolation_dt=0.02, path_buffer_last_tstep=None, debug_info=None, status=<MotionGenStatus.IK_FAIL: 'IK Fail'>, attempts=1, trajopt_attempts=0, used_graph=False, graph_plan=None, goalset_index=None)
2025-09-29 06:38:33 [Info] [isaacsim.core.prims.impl.articulation] Articulation Prim View Device: cpu
2025-09-29 06:38:33 [Info] [isaacsim.core.prims.impl.single_articulation] initializing handles for /World/franka

Regards,
Abhijit.

[Abhijit16]

The scene looks like this:

The usd file for this scene is attached:
franka.zip