Hello, I’m quite new to Isaac Sim and just learned it for about two weeks. I am trying to control a Fetch robot with Lula IK solver to accomplish a pick and place task, and at first I want to control the arm of Fetch with base fixed. I am trying to run the above task in the Hello World extension by modifying the setup_scene, setup_post_load and send_robot_actions method.
Only 8 of the joint of Fetch is controlled in my case whose indices are [2, 4, 6, 7, 8, 9, 10, 11], and I can get the target joint configurations with lula.compute_ik_ccd. However, when I try to set the joint positons by dynamic_control.set_articulation_dof_position_targets, I find that only parts of the joint is moved, while I expect all of the joints should move a little since all elements in target joint configuration is different from the initial ones. And for the result, the gripper_link (I set it as the end effector) does not stop at the correct goal position. I wonder if I deploy the joint configurations in the corrent way, or the problem is in the IK solver configuration. Here is some of my code:
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, .8]),
))
async def setup_post_load(self):
self._world = self.get_world()
self._fetchbot = self._world.scene.get_object("fetchbot")
self._world.add_physics_callback("sending_actions", callback_fn=self.send_robot_actions)
self._dc = _dynamic_control.acquire_dynamic_control_interface()
self._fetchbot_handle = self._dc.get_articulation("/World/Fetch_Robot")
self._dc.wake_up_articulation(self._fetchbot_handle)
default_joint_pos=self._dc.get_articulation_dof_states(self._fetchbot_handle, _dynamic_control.STATE_POS)["pos"]
self._fetchbot_control_idx = np.concatenate([np.array([2]), np.array([4, 6, 7, 8, 9, 10, 11])])
self._fetchbot_controller = IKSolver(
robot_description_path="/root/fetch/fetch_descriptor.yaml",
robot_urdf_path="/root/fetch/fetch.urdf",
default_joint_pos=default_joint_pos[self._fetchbot_control_idx],
eef_name="gripper_link",
)
def send_robot_actions(self, step_size):
target_pos = np.array(targets[target_index])
joint_pos = self._fetchbot_controller.solve(
target_pos=np.array([0.5, 0.5, 0.1]),
)
if joint_pos is not None:
dof_states = self._dc.get_articulation_dof_states(self._fetchbot_handle, _dynamic_control.STATE_POS)
new_positions = dof_states["pos"]
new_positions[self._fetchbot_control_idx] = joint_pos
dof_states["pos"] = new_positions
self._dc.set_articulation_dof_position_targets(self._fetchbot_handle, new_positions.astype(np.float32))
else:
carb.log_warn("fetchbot: IK did not converge to a solution. No action is being taken.")
class IKSolver:
"""
Class for thinly wrapping Lula IK solver
"""
def __init__(
self,
robot_description_path,
robot_urdf_path,
eef_name,
default_joint_pos,
):
# Create robot description, kinematics, and config
self.robot_description = lula.load_robot(robot_description_path, robot_urdf_path)
self.kinematics = self.robot_description.kinematics()
self.config = lula.CyclicCoordDescentIkConfig()
self.eef_name = eef_name
self.default_joint_pos = default_joint_pos
def solve(
self,
target_pos,
target_quat=None,
tolerance_pos=0.002,
tolerance_quat=0.01,
weight_pos=1.0,
weight_quat=0.05,
max_iterations=150,
initial_joint_pos=None,
):
"""
Backs out joint positions to achieve desired @target_pos and @target_quat
Args:
target_pos (3-array): desired (x,y,z) local target cartesian position in robot's base coordinate frame
target_quat (4-array or None): If specified, desired (x,y,z,w) local target quaternion orientation in
robot's base coordinate frame. If None, IK will be position-only (will override settings such that
orientation's tolerance is very high and weight is 0)
tolerance_pos (float): Maximum position error (L2-norm) for a successful IK solution
tolerance_quat (float): Maximum orientation error (per-axis L2-norm) for a successful IK solution
weight_pos (float): Weight for the relative importance of position error during CCD
weight_quat (float): Weight for the relative importance of position error during CCD
max_iterations (int): Number of iterations used for each cyclic coordinate descent.
initial_joint_pos (None or n-array): If specified, will set the initial cspace seed when solving for joint
positions. Otherwise, will use self.default_joint_pos
Returns:
None or n-array: Joint positions for reaching desired target_pos and target_quat, otherwise None if no
solution was found
"""
pos = np.array(target_pos, dtype=np.float64).reshape(3, 1)
rot = np.array(quats_to_rot_matrices(np.array([0, 0, 0, 1.0]) if target_quat is None else target_quat), dtype=np.float64)
ik_target_pose = lula.Pose3(lula.Rotation3(rot), pos)
# Set the cspace seed and tolerance
initial_joint_pos = self.default_joint_pos if initial_joint_pos is None else np.array(initial_joint_pos)
self.config.cspace_seeds = [initial_joint_pos]
self.config.position_tolerance = tolerance_pos
self.config.orientation_tolerance = 100.0 if target_quat is None else tolerance_quat
self.config.position_weight = weight_pos
self.config.orientation_weight = 0.0 if target_quat is None else weight_quat
self.config.max_iterations_per_descent = max_iterations
# Compute target joint positions
ik_results = lula.compute_ik_ccd(self.kinematics, ik_target_pose, self.eef_name, self.config)
return np.array(ik_results.cspace_position)