I create an simple IK solver class based on Franka’s FollowTarget task.
from omni.isaac.examples.base_sample import BaseSample
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.wheeled_robots.robots import WheeledRobot
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.controllers import BaseController
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.motion_generation import ArticulationKinematicsSolver, interface_config_loader, LulaKinematicsSolver
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.objects.cuboid import VisualCuboid
from omni.isaac.core.objects import DynamicCuboid
from typing import Optional
from omni.isaac.core.utils.rotations import euler_angles_to_quat
from omni.isaac.core.utils.numpy.rotations import rot_matrices_to_quats
import lula
import carb
from omni.isaac.franka import Franka
from omni.isaac.franka.controllers import PickPlaceController
import numpy as np
class KinematicsSolver(ArticulationKinematicsSolver):
"""Kinematics Solver for Franka robot. This class loads a LulaKinematicsSovler object
Args:
robot_articulation (Articulation): An initialized Articulation object representing this Franka
end_effector_frame_name (Optional[str]): The name of the Franka end effector. If None, an end effector link will
be automatically selected. Defaults to None.
"""
def __init__(self,
robot_description_path: str,
urdf_path: str,
robot_articulation: Articulation,
end_effector_frame_name: Optional[str] = None,
default_joint_pos: np.ndarray = None,
) -> None:
self._kinematics = LulaKinematicsSolver(robot_description_path, urdf_path)
all_frames = self._kinematics.get_all_frame_names()
all_joints = self._kinematics.get_joint_names()
carb.log_warn(f"all frames: {all_frames}\nall joints: {all_joints}")
if end_effector_frame_name is None:
end_effector_frame_name = "gripper_link"
ArticulationKinematicsSolver.__init__(self, robot_articulation, self._kinematics, end_effector_frame_name)
self._kinematics.set_max_iterations(150)
return
def set_robot_base_pose(self, robot_position: np.array, robot_orientation: np.array):
return self._kinematics.set_robot_base_pose(robot_position=robot_position, robot_orientation=robot_orientation)
I want to control the end effector to move to a position relative to the world in the following extension. It works as expected for Franka, but for Fetch Robot, the end effector always stops at a position different from the target when the action is applied. I used a visualize cube to mark the target position in the scene.
However, according to the output of compute_end_effector_pose, the end effector of Fetch is right at the target position when it stops, which is very weird.
class HelloWorld(BaseSample):
def __init__(self) -> None:
super().__init__()
return
def setup_scene(self):
world = self.get_world()
world.scene.add_default_ground_plane()
fetch_robot_path = "/fetch.usd"
fetch_robot_prim_path = "/World/Fetch_Robot"
add_reference_to_stage(usd_path=fetch_robot_path, prim_path=fetch_robot_prim_path)
self._fetchbot = world.scene.add(Robot(
prim_path=fetch_robot_prim_path,
name="fetchbot",
position=np.array([.0, .0, 0.99]),
orientation=euler_angles_to_quat([0, 0, 0]),
scale=np.array([1., 1., 1.]),
# usd_path=fetch_robot_path,
# create_robot=True,
# wheel_dof_names=["l_wheel_joint", "r_wheel_joint"],
))
self._franka = world.scene.add(Franka(
prim_path="/World/franka",
name="franka",
# position = np.array([-2., 1., 1.])
position = np.array([0., .5, .0]),
))
self._dynamic_cube = world.scene.add(DynamicCuboid(
prim_path = "/World/random_cube",
name = "fancy_cube",
position = np.array([ -1, -1, 1.]),
color = np.array([ 0, 1., 1.]),
size = .1,
))
return
async def setup_post_load(self):
self._world = self.get_world()
self._jetbot = self._world.scene.get_object("fancy_robot")
self._franka = self._world.scene.get_object("franka")
self._fetchbot = self._world.scene.get_object("fetchbot")
self._world.add_physics_callback("sending_actions", callback_fn=self.send_robot_actions)
self._franka_controller = KinematicsSolver(
robot_description_path="/isaac-sim/exts/omni.isaac.motion_generation/motion_policy_configs/franka/rmpflow/robot_descriptor.yaml",
urdf_path="/isaac-sim/exts/omni.isaac.motion_generation/motion_policy_configs/franka/lula_franka_gen.urdf",
robot_articulation=self._franka,
end_effector_frame_name="right_gripper",
)
self._franka_articulation_controller = self._franka.get_articulation_controller()
carb.log_warn("ik controller and articulation controller initialized for franka")
self._fetchbot_controller = KinematicsSolver(
robot_description_path="/root/fetch/fetch_descriptor.yaml",
urdf_path="/root/fetch/fetch.urdf",
robot_articulation=self._fetchbot,
end_effector_frame_name="gripper_link",
)
self._fetchbot_articulation_controller = self._fetchbot.get_articulation_controller()
carb.log_warn("ik controller and articulation controller initialized for fetchbot")
robot_base_translation,robot_base_orientation = self._fetchbot.get_world_pose()
self._fetchbot_controller.set_robot_base_pose(
robot_position=robot_base_translation,
robot_orientation=robot_base_orientation,
)
ee_position,ee_rot_mat = self._fetchbot_controller.compute_end_effector_pose()
ee_orientation = rot_matrices_to_quats(ee_rot_mat)
# Visulize ee pose.
self._ee_visual_cube = VisualCuboid(
prim_path="/World/ee_visual_cube",
position=ee_position,
orientation=ee_orientation,
scale=np.array([.05,.1,.05]),
color=np.array([0,0,1]),
)
self._franka_gripper_orientation = euler_angles_to_quat(np.array([0, np.pi, np.pi]))
self._iterate_count = 0
return
def send_robot_actions(self, step_size):
self._iterate_count +=1
if self._iterate_count < 10:
return
# Take a target from targets based on _iterate_count.
targets = [[0.4, -0.3, 1.4], [0.4, -0.3, 1.0], [0.4, 0, 1.0], [0.4, 0, 2.0]]
robot_base_translation,robot_base_orientation = self._fetchbot.get_world_pose()
self._fetchbot_controller.set_robot_base_pose(robot_base_translation,robot_base_orientation)
target_idx = int(self._iterate_count / 500)
if target_idx > len(targets) - 1:
target_idx = len(targets) - 1
target = np.array(targets[target_idx])
# fetchbot control.
actions_fetchbot, succ_fetchbot = self._fetchbot_controller.compute_inverse_kinematics(
target_position=target,
position_tolerance=.001,
)
if succ_fetchbot:
self._fetchbot_articulation_controller.apply_action(actions_fetchbot)
else:
carb.log_warn(f"fetchbot: IK did not converge to a solution. No action is being taken.")
ee_pose = self._fetchbot_controller.compute_end_effector_pose()
self._ee_visual_cube.set_world_pose(position=ee_pose[0], orientation=rot_matrices_to_quats(ee_pose[1]))
if self._iterate_count % 100 == 0:
carb.log_warn(f"fetchbot: pos is {robot_base_translation}, ori is {robot_base_orientation}")
carb.log_warn(f"eef: current target_pos is {target}, ee_pos is {ee_pose[0]}, ee_ori={rot_matrices_to_quats(ee_pose[1])}")
dynamic_cube_pose = self._dynamic_cube.get_world_pose()
carb.log_warn(f"dynamic cube pose: {dynamic_cube_pose[0]}, {dynamic_cube_pose[1]}")
# franka control.
robot_base_translation,robot_base_orientation = self._franka.get_world_pose()
self._franka_controller.set_robot_base_pose(robot_base_translation,robot_base_orientation)
actions_franka, succ_franka = self._franka_controller.compute_inverse_kinematics(
target_position=np.array([.4, .4, .1]),
target_orientation=self._franka_gripper_orientation,
position_tolerance=.01,
orientation_tolerance=.01,
)
if succ_franka:
self._franka_articulation_controller.apply_action(actions_franka)
else:
carb.log_warn("franka: IK did not converge to a solution. No action is being taken.")
return
After some test, I found one thing that may be related: the Fetch Robot’s position from get_world_pose(robot_base_translation in my code) always has a non-zero z-axis offset while it is on the ground, which equals almost exactly to the deviation of its end effector from target position. I expect it has a near-zero z-axis value when it’s on the ground.
So is it the reason why the end effector is not at the target position? And how can I solve this problem
Here is the usd, urdf and description file I used:
fetch.zip (6.1 MB)
