Compute Inverse kinematics using LulaKinematicsSolver in RL library

[swimpark]

Hi ! @toni.sm

I am customizing my RL env in IsaacSim. (Cannot use RL Framework by some issues :,| )

I want to compute inverse kinematics using LulaKinematicsSolver in RL library.

I used LulaKinematicsSolver class in my python code as below.

PATH/ov/pkg/isaac_sim2022.2.0/standalone_examples/api/omni.isaac.gym/robot_task.py

   def set_up_scene(self, scene) -> None:
        ...
        # Create Target point
        self._target_position = [random.uniform(0.4, 0.6), random.uniform(-0.3, 0.3), random.uniform(0.75, 1.2)]
        create_prim(prim_path="/World/Target", prim_type="Xform", position=self._target_position)
        
        # Create an ArticulationView wrapper for our robot - this can be extended towards accessing multiple robots
        self._robots = ArticulationView(prim_paths_expr="/World/M1013*", name="robot_view")
        
        # Add Robot ArticulationView and ground plane to the Scene
        scene.add(self._robots)
        scene.add_default_ground_plane()
        scene.get_object(self._robots)

        # Add LulaKinematicsSolver
        lula_kinematics_solver = LulaKinematicsSolver(
            robot_description_path="/PATH/urdf/rmpflow/robot_descriptor.yaml",
            urdf_path="/PATH/urdf/robot.urdf",
        )
        end_effector_name = "Mount"
        my_robot=scene.get_object("/World/M1013")
        self.articulation_kinematics_solver = ArticulationKinematicsSolver(my_robot,lula_kinematics_solver,end_effector_name)
   
   ...

   def pre_physics_step(self, actions) -> None:  
        ...
        # compute IK solution for the current point
        actions, succ = self.articulation_kinematics_solver.compute_inverse_kinematics(
            target_position=get_prim_property(prim_path="/World/Target", property_name="xformOp:translate"),
            target_orientation=np.array([0, 0, 1, 0]),  # arbitrary orientation
        )
        if succ:
            print("Actions result : ", actions)
            pass
        else:
            carb.log_warn("IK did not converge for IK point.")
         ...

However, there are some errors like below.

Traceback (most recent call last):
  File "robot_train.py", line 36, in <module>
    model.learn(total_timesteps=1000000)
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/kit/python/lib/python3.7/site-packages/stable_baselines3/ppo/ppo.py", line 327, in learn
    progress_bar=progress_bar,
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/kit/python/lib/python3.7/site-packages/stable_baselines3/common/on_policy_algorithm.py", line 262, in learn
    continue_training = self.collect_rollouts(self.env, callback, self.rollout_buffer, n_rollout_steps=self.n_steps)
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/kit/python/lib/python3.7/site-packages/stable_baselines3/common/on_policy_algorithm.py", line 181, in collect_rollouts
    new_obs, rewards, dones, infos = env.step(clipped_actions)
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/kit/python/lib/python3.7/site-packages/stable_baselines3/common/vec_env/base_vec_env.py", line 162, in step
    return self.step_wait()
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/kit/python/lib/python3.7/site-packages/stable_baselines3/common/vec_env/dummy_vec_env.py", line 44, in step_wait
    self.actions[env_idx]
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/kit/python/lib/python3.7/site-packages/stable_baselines3/common/monitor.py", line 94, in step
    observation, reward, done, info = self.env.step(action)
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/exts/omni.isaac.gym/omni/isaac/gym/vec_env/vec_env_base.py", line 133, in step
    self._task.pre_physics_step(actions)
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/standalone_examples/api/omni.isaac.gym/Kcloud/robot_task.py", line 161, in pre_physics_step
    target_orientation=np.array([0, 0, 1, 0]),  # arbitrary orientation
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/exts/omni.isaac.motion_generation/omni/isaac/motion_generation/articulation_kinematics_solver.py", line 85, in compute_inverse_kinematics
    warm_start = self._joints_view.get_joint_positions()
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/exts/omni.isaac.core/omni/isaac/core/articulations/articulation_subset.py", line 25, in decorator
    if not self.is_initialized:
  File "/home/idim/.local/share/ov/pkg/isaac_sim-2022.2.0/exts/omni.isaac.core/omni/isaac/core/articulations/articulation_subset.py", line 68, in is_initialized
    return self.articulation.handles_initialized
AttributeError: 'NoneType' object has no attribute 'handles_initialized'
2023-03-17 11:58:42 [15,268ms] [Warning] [carb.audio.context] 1 contexts were leaked

Is it possible to implement the computation of IK in this case ?

Thank you !

[toni.sm]

Hi @swimpark

First,

@swimpark

I am customizing my RL env in IsaacSim. (Cannot use RL Framework by some issues :,| )

I really recommend you to move to the OmniIsaacGymEnvs RL Framework if you want to use massively parallel simulation.

---

I think the problem is that "/World/M1013" is not registered as an Articulation (or Robot) object.

@swimpark

my_robot=scene.get_object("/World/M1013")

Then, the line above is returning a NoneType object.
By the way, scene.get_object expect an Isaac Core object name rather than a prim path.

Wrap that prim with an Articulation (or Robot) class, and add the to the scene before passing it to the ArticulationKinematicsSolver

---

I think the LulaKinematicsSolver doesn’t support views and tensor API, so it will not be able to be used in parallel environments.

Have you tried differential inverse kinematic for RL.
In the case you will implementing your Rl environment in Isaac Orbit, visit Differential inverse kinematics (IK).
For OmniIsaacGymEnvs, you can see an example case in the skrl’s Real-world examples.

[swimpark]

Hi @toni.sm !

Thank you very much your kind reply, always.

I changed my env to OmniIsaacGymEnvs as you recommended, and implement the example(skrl’s robot control with my robot.

I have two questions.

1. Different success rate btw two robot models.
   [Case: Only Robot arm]
   
   
   [Case: Mobile base + robot arm]
   

I trained two different robot with the same train python file, and the second case (with mobile base) acts worse than the first case.

The second robot was only added with ‘fixed joint’ and some links based on the first robot.

Do you know why the second robot model acts worse ?

2. Exact meaning of ‘jacobian_end_effector’ parameter

    def pre_physics_step(self, actions) -> None:
       ...
        elif self._control_space == "cartesian":
            goal_position = self.end_effector_pos + actions / 100.0
            delta_dof_pos = omniverse_isaacgym_utils.ik(jacobian_end_effector=self.jacobians[:, 6 - 1, :, :6],  # link_6 index: 6
                                                        current_position=self.end_effector_pos,
                                                        current_orientation=self.end_effector_rot,
                                                        goal_position=goal_position,
                                                        goal_orientation=None)
            targets = self.robot_dof_targets[:, :6] + delta_dof_pos
        ...

In the example code (reaching_xxx_omniverse_isaacgym_env.py), it is quite difficult to understand the exact meaning of the parameter ‘jacobian_end_effector’.

Could you explain the four index values of jacobian_end_effector ?

Thank you !

[toni.sm]

Hi @swimpark

At first glance, I can’t think of what might be the cause of the problem. Perhaps it is related to the fixed joint which, if not excluded, modifies the structure of the joint.

Refer to the Isaac Gym Preview documentation under Programming > Tensor API > Physics State > Jacobians and Mass Matrices for details about the Jacobian matrix and its use.

[swimpark]

Hi @toni.sm , thank you for your kind reply.

I modified my the values in jacobian() as you mentioned, but it didn’t match to my case.

The document that you showed me says the input values for jacobian of a fixed robot arm as the below.

jacobian[env_index, link_index -1, :, dof_index]

Then, I checked the information of my robot with the code below, and confirmed the indexing error.

print("Dof number :",  self._robots.num_dof, "Link(link6) index :",  self._robots.get_body_index("link6"), "joint(joint6) index :",  self._robots.get_dof_index("joint6"))
print("\n Jacobian Matrix :", self.jacobians[0, 8 - 1, :, :6] )

#Result
Dof number : 6 / Link(link6) index : 8 / joint(joint6) index : 5 
IndexError: index 7 is out of bounds for dimension 1 with size 6

In my case, I set [end effector : “link6”] and [the last revolution joint : “joint6”].

The links, [“world”,“AMR”, “link6”, “Mount”, “D415”] have just fixed joints.

idim_m1013.usd (68.5 MB)

Could you let me know how train the robot control with jacobian matrix properly ?

Thank you !

[philipp.reist]

Hi @swimpark,

I am also not sure what’s going wrong, but maybe this will help:

If you do not exclude the AMR2ARM fixed joint between the mobile base and the arm from the articulation, you may get one single articulation for the mobile robot + arm. In that case, assuming you will make the mobile base not fixed, you will have a floating-base articulation where you will get the extra jacobian entries for the root (just identity anyway).

For simulation fidelity, this is the preferred setup, actually - a single articulation. For IK, you would have to then identify the entries that correspond to the EE taking the mobile base into account.

Another approach is that you exclude the fixed joint AMR2ARM from the articulation (there is a check-mark) and you will get the mobile base and the arm as separate simulation objects. In that case, the arm will again be a floating base articulation and you’ll have to take that into account when indexing into the jacobian.

Hope this helps,
Philipp